diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index 47223a5d..b7b7b92a 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -81,7 +81,6 @@ void handleRobotProgramState(bool program_running) } // Callback function for trajectory execution. -// control::TrajectoryResult trajectory_state(TRAJECTORY_RESULT_CANCELED); bool g_trajectory_running(false); void handleTrajectoryState(control::TrajectoryResult state) { @@ -225,8 +224,8 @@ int main(int argc, char* argv[]) // CUBIC SendTrajectory(p, v, std::vector(), time, true); - trajectory_running = true; - while (trajectory_running) + g_trajectory_running = true; + while (g_trajectory_running) { std::unique_ptr data_pkg = g_my_driver->getDataPackage(); if (data_pkg) @@ -238,9 +237,7 @@ int main(int argc, char* argv[]) std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; throw std::runtime_error(error_msg); } - // g_joint_positions[0] = s_pos[0]; // Move to initial position of the spline bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); - // bool ret = g_my_driver->writeKeepalive(); if (!ret) { @@ -256,10 +253,9 @@ int main(int argc, char* argv[]) SendTrajectory(p, v, a, time, true); ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); - trajectory_running = true; - while (trajectory_running) + g_trajectory_running = true; + while (g_trajectory_running) { - // g_my_driver->getDataPackage(); std::unique_ptr data_pkg = g_my_driver->getDataPackage(); if (data_pkg) { @@ -271,7 +267,6 @@ int main(int argc, char* argv[]) throw std::runtime_error(error_msg); } bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); - // bool ret = g_my_driver->writeKeepalive(); if (!ret) { diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 235ace4d..1f5e50ff 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -54,7 +54,7 @@ enum class TrajectoryResult : int32_t */ enum class TrajectorySplineType : int32_t { - SPLINE_QUADRATIC = 0. + SPLINE_QUADRATIC = 0, SPLINE_CUBIC = 1, SPLINE_QUINTIC = 2 }; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3cbdc869..dc194537 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -55,8 +55,9 @@ FREEDRIVE_MODE_STOP = -1 UNTIL_TOOL_CONTACT_RESULT_SUCCESS = 0 UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1 -CUBIC_SPLINES = 0 -QUINTIC_SPLINES = 1 +SPLINE_QUADRATIC = 0 # Not implemented yet +SPLINE_CUBIC = 1 +SPLINE_QUINTIC = 2 #Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED @@ -242,7 +243,7 @@ thread trajectoryThread(): # Joint spline point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: # Cubic spline - if raw_point[INDEX_SPLINE_TYPE] == CUBIC_SPLINES: + if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate cubicSplineRun(q, qd, tmptime) @@ -250,7 +251,7 @@ thread trajectoryThread(): last_spline_qdd = [0, 0, 0, 0, 0, 0] # Quintic spline - elif raw_point[INDEX_SPLINE_TYPE] == QUINTIC_SPLINES: + elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: qd = [ raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate qdd = [ raw_point[13], raw_point[14], raw_point[15], raw_point[16], raw_point[17], raw_point[18]] / MULT_jointstate quinticSplineRun(q, qd, qdd, tmptime) @@ -264,9 +265,9 @@ thread trajectoryThread(): movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=0.0) elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: # Cubic spline - if raw_point[INDEX_SPLINE_TYPE] == CUBIC_SPLINES: + if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: cubicSplineRun(q, [0,0,0,0,0,0], tmptime) - elif raw_point[INDEX_SPLINE_TYPE] == QUINTIC_SPLINES: + elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: quinticSplineRun(q, [0,0,0,0,0,0], [0,0,0,0,0,0], tmptime) end end