19 #include "../../include/robotis_manipulator/robotis_manipulator_trajectory_generator.h" 34 Eigen::Matrix3d A = Eigen::Matrix3d::Identity(3, 3);
35 Eigen::Vector3d x = Eigen::Vector3d::Zero();
36 Eigen::Vector3d b = Eigen::Vector3d::Zero();
38 A << pow(move_time, 3), pow(move_time, 4), pow(move_time, 5),
39 3 * pow(move_time, 2), 4 * pow(move_time, 3), 5 * pow(move_time, 4),
40 6 * pow(move_time, 1), 12 * pow(move_time, 2), 20 * pow(move_time, 3);
50 Eigen::ColPivHouseholderQR<Eigen::Matrix3d> dec(A);
73 coefficient_size_ = start.size();
74 minimum_jerk_coefficient_.resize(6,coefficient_size_);
75 for (uint8_t index = 0; index < coefficient_size_; index++)
77 minimum_jerk_trajectory_generator_.calcCoefficient(start.at(index),
81 minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
89 for (uint8_t index = 0; index < coefficient_size_; index++)
93 single_joint_way_point.
position = minimum_jerk_coefficient_(0, index) +
94 minimum_jerk_coefficient_(1, index) * pow(tick, 1) +
95 minimum_jerk_coefficient_(2, index) * pow(tick, 2) +
96 minimum_jerk_coefficient_(3, index) * pow(tick, 3) +
97 minimum_jerk_coefficient_(4, index) * pow(tick, 4) +
98 minimum_jerk_coefficient_(5, index) * pow(tick, 5);
100 single_joint_way_point.
velocity = minimum_jerk_coefficient_(1, index) +
101 2 * minimum_jerk_coefficient_(2, index) * pow(tick, 1) +
102 3 * minimum_jerk_coefficient_(3, index) * pow(tick, 2) +
103 4 * minimum_jerk_coefficient_(4, index) * pow(tick, 3) +
104 5 * minimum_jerk_coefficient_(5, index) * pow(tick, 4);
106 single_joint_way_point.
acceleration = 2 * minimum_jerk_coefficient_(2, index) +
107 6 * minimum_jerk_coefficient_(3, index) * pow(tick, 1) +
108 12 * minimum_jerk_coefficient_(4, index) * pow(tick, 2) +
109 20 * minimum_jerk_coefficient_(5, index) * pow(tick, 3);
111 single_joint_way_point.
effort = 0.0;
113 joint_way_point.push_back(single_joint_way_point);
116 return joint_way_point;
121 return minimum_jerk_coefficient_;
128 minimum_jerk_coefficient_ = Eigen::MatrixXd::Identity(6, 4);
135 std::vector<Point> start_way_point;
136 std::vector<Point> goal_way_point;
139 for(uint8_t i = 0; i < 3; i++)
145 position_temp.
effort = 0.0;
146 start_way_point.push_back(position_temp);
151 position_temp.
effort = 0.0;
152 goal_way_point.push_back(position_temp);
158 Eigen::Vector3d start_orientation_rpy;
159 Eigen::Vector3d start_ang_vel_rpy;
160 Eigen::Vector3d start_ang_acc_rpy;
166 Eigen::Vector3d goal_orientation_rpy;
167 Eigen::Vector3d goal_ang_vel_rpy;
168 Eigen::Vector3d goal_ang_acc_rpy;
174 for(uint8_t i = 0; i < 3; i++)
176 Point orientation_temp;
177 orientation_temp.
position = start_orientation_rpy[i];
178 orientation_temp.
velocity = start_ang_vel_rpy[i];
180 orientation_temp.
effort = 0.0;
181 start_way_point.push_back(orientation_temp);
183 orientation_temp.
position = goal_orientation_rpy[i];
184 orientation_temp.
velocity = goal_ang_vel_rpy[i];
186 orientation_temp.
effort = 0.0;
187 goal_way_point.push_back(orientation_temp);
191 coefficient_size_ = start_way_point.size();
192 minimum_jerk_coefficient_.resize(6,coefficient_size_);
193 for (uint8_t index = 0; index < coefficient_size_; index++)
195 minimum_jerk_trajectory_generator_.calcCoefficient(start_way_point.at(index),
196 goal_way_point.at(index),
199 minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
206 std::vector<Point> result_point;
207 for (uint8_t index = 0; index < coefficient_size_; index++)
209 Point single_task_way_point;
211 single_task_way_point.
position = minimum_jerk_coefficient_(0, index) +
212 minimum_jerk_coefficient_(1, index) * pow(tick, 1) +
213 minimum_jerk_coefficient_(2, index) * pow(tick, 2) +
214 minimum_jerk_coefficient_(3, index) * pow(tick, 3) +
215 minimum_jerk_coefficient_(4, index) * pow(tick, 4) +
216 minimum_jerk_coefficient_(5, index) * pow(tick, 5);
218 single_task_way_point.
velocity = minimum_jerk_coefficient_(1, index) +
219 2 * minimum_jerk_coefficient_(2, index) * pow(tick, 1) +
220 3 * minimum_jerk_coefficient_(3, index) * pow(tick, 2) +
221 4 * minimum_jerk_coefficient_(4, index) * pow(tick, 3) +
222 5 * minimum_jerk_coefficient_(5, index) * pow(tick, 4);
224 single_task_way_point.
acceleration = 2 * minimum_jerk_coefficient_(2, index) +
225 6 * minimum_jerk_coefficient_(3, index) * pow(tick, 1) +
226 12 * minimum_jerk_coefficient_(4, index) * pow(tick, 2) +
227 20 * minimum_jerk_coefficient_(5, index) * pow(tick, 3);
229 single_task_way_point.
effort = 0.0;
231 result_point.push_back(single_task_way_point);
236 for(uint8_t i = 0; i < 3; i++)
245 Eigen::Vector3d rpy_orientation;
246 rpy_orientation << result_point.at(3).position, result_point.at(4).position, result_point.at(5).position;
248 result_point.at(4).position,
249 result_point.at(5).position);
251 Eigen::Vector3d rpy_velocity;
252 rpy_velocity << result_point.at(3).velocity, result_point.at(4).velocity, result_point.at(5).velocity;
255 Eigen::Vector3d rpy_acceleration;
256 rpy_acceleration << result_point.at(3).acceleration, result_point.at(4).acceleration, result_point.at(5).acceleration;
259 return task_way_point;
264 return minimum_jerk_coefficient_;
273 trajectory_time_.total_move_time = move_time;
278 trajectory_time_.present_time = present_time;
283 trajectory_time_.start_time = trajectory_time_.present_time;
288 trajectory_time_.start_time = start_time;
293 return trajectory_time_.total_move_time;
298 return trajectory_time_.present_time - trajectory_time_.start_time;
303 manipulator_= manipulator;
308 return &manipulator_;
323 return cus_joint_.at(name);
328 return cus_task_.at(name);
333 cus_joint_.insert(std::make_pair(trajectory_name, custom_trajectory));
338 cus_task_.insert(std::make_pair(trajectory_name, custom_trajectory));
343 if(cus_joint_.find(trajectory_name) != cus_joint_.end())
344 cus_joint_.at(trajectory_name)->setOption(arg);
345 else if(cus_task_.find(trajectory_name) != cus_task_.end())
346 cus_task_.at(trajectory_name)->setOption(arg);
351 present_control_tool_name_ = present_control_tool_name;
356 return present_custom_trajectory_name_;
361 return present_control_tool_name_;
366 setManipulator(actual_manipulator);
368 joint_way_point_vector = getManipulator()->getAllActiveJointValue();
370 setPresentJointWaypoint(joint_way_point_vector);
371 if(kinematics !=
nullptr)
372 updatePresentWaypoint(kinematics);
383 manipulator_.setAllActiveJointValue(joint_value_vector);
388 manipulator_.setComponentPoseFromWorld(tool_name, tool_value_vector);
393 return manipulator_.getAllActiveJointValue();
398 return manipulator_.getComponentPoseFromWorld(tool_name);
403 for(uint32_t index =0; index < value.size(); index++)
405 value.at(index).velocity = 0.0;
406 value.at(index).acceleration = 0.0;
407 value.at(index).effort = 0.0;
425 trajectory_type_ = trajectory_type;
430 if(trajectory_type_==trajectory_type)
438 return joint_.makeJointTrajectory(trajectory_time_.total_move_time, start_way_point, goal_way_point);
443 return task_.makeTaskTrajectory(trajectory_time_.total_move_time, start_way_point, goal_way_point);
448 if(cus_joint_.find(trajectory_name) != cus_joint_.end())
450 present_custom_trajectory_name_ = trajectory_name;
451 cus_joint_.at(trajectory_name)->makeJointTrajectory(trajectory_time_.total_move_time, start_way_point, arg);
456 log::error(
"[makeCustomTrajectory] Wrong way point type.");
463 if(cus_task_.find(trajectory_name) != cus_task_.end())
465 present_custom_trajectory_name_ = trajectory_name;
466 cus_task_.at(trajectory_name)->makeTaskTrajectory(trajectory_time_.total_move_time, start_way_point, arg);
471 log::error(
"[makeCustomTrajectory] Wrong way point type.");
479 manipulator_.setJointPosition(tool_name, tool_goal_position);
486 manipulator_.setJointValue(tool_name, tool_goal_value);
492 return manipulator_.getJointPosition(tool_name);
497 return manipulator_.getJointValue(tool_name);
bool makeJointTrajectory(JointWaypoint start_way_point, JointWaypoint goal_way_point)
makeJointTrajectory
Name getPresentCustomTrajectoryName()
void setManipulator(Manipulator manipulator)
void setPresentJointWaypoint(JointWaypoint joint_value_vector)
bool setToolGoalValue(Name tool_name, JointValue tool_goal_value)
setToolGoalValue
CustomTaskTrajectory * getCustomTaskTrajectory(Name name)
enum robotis_manipulator::_TrajectoryType TrajectoryType
void setStartTimeToPresentTime()
virtual void solveForwardKinematics(Manipulator *manipulator)=0
void setPresentControlToolName(Name present_control_tool_name)
virtual ~JointTrajectory()
void addCustomTrajectory(Name trajectory_name, CustomJointTrajectory *custom_trajectory)
void calcCoefficient(Point start, Point goal, double move_time)
void initTrajectoryWaypoint(Manipulator actual_manipulator, Kinematics *kinematics=nullptr)
Eigen::MatrixXd getMinimumJerkCoefficient()
bool checkTrajectoryType(TrajectoryType trajectory_type)
Eigen::VectorXd getCoefficient()
TaskTrajectory getTaskTrajectory()
Eigen::Vector3d convertRPYVelocityToOmega(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
void setPresentTaskWaypoint(Name tool_name, TaskWaypoint tool_position_value_vector)
void setTrajectoryType(TrajectoryType trajectory_type)
JointValue getToolGoalValue(Name tool_name)
CustomJointTrajectory * getCustomJointTrajectory(Name name)
std::vector< JointValue > JointWaypoint
bool makeCustomTrajectory(Name trajectory_name, JointWaypoint start_way_point, const void *arg)
makeCustomTrajectory
void updatePresentWaypoint(Kinematics *kinematics)
Name getPresentControlToolName()
Eigen::Vector3d convertRPYAccelerationToOmegaDot(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
void setMoveTime(double move_time)
Manipulator * getManipulator()
void setPresentTime(double present_time)
JointWaypoint getPresentJointWaypoint()
JointWaypoint getJointWaypoint(double tick)
Eigen::VectorXd coefficient_
Eigen::Vector3d convertOmegaToRPYVelocity(Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
double getToolGoalPosition(Name tool_name)
getToolGoalPosition
Eigen::MatrixXd getMinimumJerkCoefficient()
bool makeTaskTrajectory(TaskWaypoint start_way_point, TaskWaypoint goal_way_point)
makeTaskTrajectory
TaskWaypoint getTaskWaypoint(double tick)
JointTrajectory getJointTrajectory()
Eigen::Vector3d acceleration
bool setToolGoalPosition(Name tool_name, double tool_goal_position)
setToolGoalPosition
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
JointWaypoint removeWaypointDynamicData(JointWaypoint value)
bool makeTaskTrajectory(double move_time, TaskWaypoint start, TaskWaypoint goal)
makeTaskTrajectory
TaskWaypoint getPresentTaskWaypoint(Name tool_name)
Eigen::Vector3d convertOmegaDotToRPYAcceleration(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
void setStartTime(double start_time)
Eigen::Matrix3d orientation
void setCustomTrajectoryOption(Name trajectory_name, const void *arg)
virtual ~TaskTrajectory()
bool makeJointTrajectory(double move_time, JointWaypoint start, JointWaypoint goal)
makeJointTrajectory
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)