26 #include "../../include/robotis_manipulator/robotis_manipulator_trajectory_generator.h" 41 Eigen::Matrix3d A = Eigen::Matrix3d::Identity(3, 3);
42 Eigen::Vector3d x = Eigen::Vector3d::Zero();
43 Eigen::Vector3d b = Eigen::Vector3d::Zero();
45 A << pow(move_time, 3), pow(move_time, 4), pow(move_time, 5),
46 3 * pow(move_time, 2), 4 * pow(move_time, 3), 5 * pow(move_time, 4),
47 6 * pow(move_time, 1), 12 * pow(move_time, 2), 20 * pow(move_time, 3);
57 Eigen::ColPivHouseholderQR<Eigen::Matrix3d> dec(A);
80 coefficient_size_ = start.size();
81 minimum_jerk_coefficient_.resize(6,coefficient_size_);
82 for (uint8_t index = 0; index < coefficient_size_; index++)
84 minimum_jerk_trajectory_generator_.calcCoefficient(start.at(index),
88 minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
95 for (uint8_t index = 0; index < coefficient_size_; index++)
99 single_joint_way_point.
position = minimum_jerk_coefficient_(0, index) +
100 minimum_jerk_coefficient_(1, index) * pow(tick, 1) +
101 minimum_jerk_coefficient_(2, index) * pow(tick, 2) +
102 minimum_jerk_coefficient_(3, index) * pow(tick, 3) +
103 minimum_jerk_coefficient_(4, index) * pow(tick, 4) +
104 minimum_jerk_coefficient_(5, index) * pow(tick, 5);
106 single_joint_way_point.
velocity = minimum_jerk_coefficient_(1, index) +
107 2 * minimum_jerk_coefficient_(2, index) * pow(tick, 1) +
108 3 * minimum_jerk_coefficient_(3, index) * pow(tick, 2) +
109 4 * minimum_jerk_coefficient_(4, index) * pow(tick, 3) +
110 5 * minimum_jerk_coefficient_(5, index) * pow(tick, 4);
112 single_joint_way_point.
acceleration = 2 * minimum_jerk_coefficient_(2, index) +
113 6 * minimum_jerk_coefficient_(3, index) * pow(tick, 1) +
114 12 * minimum_jerk_coefficient_(4, index) * pow(tick, 2) +
115 20 * minimum_jerk_coefficient_(5, index) * pow(tick, 3);
117 single_joint_way_point.
effort = 0.0;
119 joint_way_point.push_back(single_joint_way_point);
122 return joint_way_point;
127 return minimum_jerk_coefficient_;
134 minimum_jerk_coefficient_ = Eigen::MatrixXd::Identity(6, 4);
141 std::vector<Point> start_way_point;
142 std::vector<Point> goal_way_point;
145 for(uint8_t i = 0; i < 3; i++)
151 position_temp.
effort = 0.0;
152 start_way_point.push_back(position_temp);
157 position_temp.
effort = 0.0;
158 goal_way_point.push_back(position_temp);
164 Eigen::Vector3d start_orientation_rpy;
165 Eigen::Vector3d start_ang_vel_rpy;
166 Eigen::Vector3d start_ang_acc_rpy;
172 Eigen::Vector3d goal_orientation_rpy;
173 Eigen::Vector3d goal_ang_vel_rpy;
174 Eigen::Vector3d goal_ang_acc_rpy;
180 for(uint8_t i = 0; i < 3; i++)
182 Point orientation_temp;
183 orientation_temp.
position = start_orientation_rpy[i];
184 orientation_temp.
velocity = start_ang_vel_rpy[i];
186 orientation_temp.
effort = 0.0;
187 start_way_point.push_back(orientation_temp);
189 orientation_temp.
position = goal_orientation_rpy[i];
190 orientation_temp.
velocity = goal_ang_vel_rpy[i];
192 orientation_temp.
effort = 0.0;
193 goal_way_point.push_back(orientation_temp);
197 coefficient_size_ = start_way_point.size();
198 minimum_jerk_coefficient_.resize(6,coefficient_size_);
199 for (uint8_t index = 0; index < coefficient_size_; index++)
201 minimum_jerk_trajectory_generator_.calcCoefficient(start_way_point.at(index),
202 goal_way_point.at(index),
205 minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
211 std::vector<Point> result_point;
212 for (uint8_t index = 0; index < coefficient_size_; index++)
214 Point single_task_way_point;
216 single_task_way_point.
position = minimum_jerk_coefficient_(0, index) +
217 minimum_jerk_coefficient_(1, index) * pow(tick, 1) +
218 minimum_jerk_coefficient_(2, index) * pow(tick, 2) +
219 minimum_jerk_coefficient_(3, index) * pow(tick, 3) +
220 minimum_jerk_coefficient_(4, index) * pow(tick, 4) +
221 minimum_jerk_coefficient_(5, index) * pow(tick, 5);
223 single_task_way_point.
velocity = minimum_jerk_coefficient_(1, index) +
224 2 * minimum_jerk_coefficient_(2, index) * pow(tick, 1) +
225 3 * minimum_jerk_coefficient_(3, index) * pow(tick, 2) +
226 4 * minimum_jerk_coefficient_(4, index) * pow(tick, 3) +
227 5 * minimum_jerk_coefficient_(5, index) * pow(tick, 4);
229 single_task_way_point.
acceleration = 2 * minimum_jerk_coefficient_(2, index) +
230 6 * minimum_jerk_coefficient_(3, index) * pow(tick, 1) +
231 12 * minimum_jerk_coefficient_(4, index) * pow(tick, 2) +
232 20 * minimum_jerk_coefficient_(5, index) * pow(tick, 3);
234 single_task_way_point.
effort = 0.0;
236 result_point.push_back(single_task_way_point);
241 for(uint8_t i = 0; i < 3; i++)
250 Eigen::Vector3d rpy_orientation;
251 rpy_orientation << result_point.at(3).position, result_point.at(4).position, result_point.at(5).position;
253 result_point.at(4).position,
254 result_point.at(5).position);
256 Eigen::Vector3d rpy_velocity;
257 rpy_velocity << result_point.at(3).velocity, result_point.at(4).velocity, result_point.at(5).velocity;
260 Eigen::Vector3d rpy_acceleration;
261 rpy_acceleration << result_point.at(3).acceleration, result_point.at(4).acceleration, result_point.at(5).acceleration;
264 return task_way_point;
269 return minimum_jerk_coefficient_;
278 trajectory_time_.total_move_time = move_time;
283 trajectory_time_.present_time = present_time;
288 trajectory_time_.start_time = trajectory_time_.present_time;
293 trajectory_time_.start_time = start_time;
298 return trajectory_time_.total_move_time;
303 return trajectory_time_.present_time - trajectory_time_.start_time;
308 manipulator_= manipulator;
313 return &manipulator_;
328 return cus_joint_.at(name);
333 return cus_task_.at(name);
338 cus_joint_.insert(std::make_pair(trajectory_name, custom_trajectory));
343 cus_task_.insert(std::make_pair(trajectory_name, custom_trajectory));
348 if(cus_joint_.find(trajectory_name) != cus_joint_.end())
349 cus_joint_.at(trajectory_name)->setOption(arg);
350 else if(cus_task_.find(trajectory_name) != cus_task_.end())
351 cus_task_.at(trajectory_name)->setOption(arg);
356 present_control_tool_name_ = present_control_tool_name;
361 return present_custom_trajectory_name_;
366 return present_control_tool_name_;
371 setManipulator(actual_manipulator);
373 joint_way_point_vector = getManipulator()->getAllActiveJointValue();
375 setPresentJointWaypoint(joint_way_point_vector);
376 updatePresentWaypoint(kinematics);
387 manipulator_.setAllActiveJointValue(joint_value_vector);
392 manipulator_.setComponentPoseFromWorld(tool_name, tool_value_vector);
397 return manipulator_.getAllActiveJointValue();
402 return manipulator_.getComponentPoseFromWorld(tool_name);
407 for(uint32_t index =0; index < value.size(); index++)
409 value.at(index).velocity = 0.0;
410 value.at(index).acceleration = 0.0;
411 value.at(index).effort = 0.0;
429 trajectory_type_ = trajectory_type;
434 if(trajectory_type_==trajectory_type)
442 joint_.makeJointTrajectory(trajectory_time_.total_move_time, start_way_point, goal_way_point);
447 task_.makeTaskTrajectory(trajectory_time_.total_move_time, start_way_point, goal_way_point);
452 if(cus_joint_.find(trajectory_name) != cus_joint_.end())
454 present_custom_trajectory_name_ = trajectory_name;
455 cus_joint_.at(trajectory_name)->makeJointTrajectory(trajectory_time_.total_move_time, start_way_point, arg);
458 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);
469 log::error(
"[makeCustomTrajectory] Wrong way point type.");
475 manipulator_.setJointPosition(tool_name, tool_goal_position);
481 manipulator_.setJointValue(tool_name, tool_goal_value);
486 return manipulator_.getJointPosition(tool_name);
491 return manipulator_.getJointValue(tool_name);
void setPresentTaskWaypoint(Name tool_name, TaskWaypoint tool_position_value_vector)
setPresentTaskWaypoint
CustomTaskTrajectory * getCustomTaskTrajectory(Name name)
getCustomTaskTrajectory
void setPresentControlToolName(Name present_control_tool_name)
setPresentControlToolName
void setMoveTime(double move_time)
setMoveTime
void addCustomTrajectory(Name trajectory_name, CustomJointTrajectory *custom_trajectory)
addCustomTrajectory
Eigen::VectorXd coefficient_
TaskWaypoint getPresentTaskWaypoint(Name tool_name)
getPresentTaskWaypoint
void makeCustomTrajectory(Name trajectory_name, JointWaypoint start_way_point, const void *arg)
makeCustomTrajectory
Manipulator * getManipulator()
getManipulator
void calcCoefficient(Point start, Point goal, double move_time)
calcCoefficient
void setCustomTrajectoryOption(Name trajectory_name, const void *arg)
setCustomTrajectoryOption
TaskWaypoint getTaskWaypoint(double tick)
getTaskWaypoint
virtual void solveForwardKinematics(Manipulator *manipulator)=0
solveForwardKinematics
Eigen::Vector3d convertRPYVelocityToOmega(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
convertRPYVelocityToOmega
void setToolGoalPosition(Name tool_name, double tool_goal_position)
setToolGoalPosition
The CustomJointTrajectory class.
void setStartTimeToPresentTime()
setStartTimeToPresentTime
Eigen::MatrixXd getMinimumJerkCoefficient()
getMinimumJerkCoefficient
void makeTaskTrajectory(double move_time, TaskWaypoint start, TaskWaypoint goal)
makeTaskTrajectory
std::vector< JointValue > JointWaypoint
JointWaypoint getPresentJointWaypoint()
getPresentJointWaypoint
void makeJointTrajectory(double move_time, JointWaypoint start, JointWaypoint goal)
makeJointTrajectory
void setStartTime(double start_time)
setStartTime
The JointTrajectory class.
Eigen::Vector3d convertRPYAccelerationToOmegaDot(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
convertRPYAccelerationToOmegaDot
void setPresentJointWaypoint(JointWaypoint joint_value_vector)
setPresentJointWaypoint
void updatePresentWaypoint(Kinematics *kinematics)
updatePresentWaypoint
The TaskTrajectory class.
JointValue getToolGoalValue(Name tool_name)
getToolGoalValue
The CustomTaskTrajectory class.
void setToolGoalValue(Name tool_name, JointValue tool_goal_value)
setToolGoalValue
bool checkTrajectoryType(TrajectoryType trajectory_type)
checkTrajectoryType
TaskTrajectory getTaskTrajectory()
getTaskTrajectory
JointWaypoint getJointWaypoint(double tick)
getJointWaypoint
Eigen::Vector3d convertOmegaToRPYVelocity(Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
convertOmegaToRPYVelocity
Eigen::Vector3d acceleration
Eigen::MatrixXd getMinimumJerkCoefficient()
getMinimumJerkCoefficient
double getToolGoalPosition(Name tool_name)
getToolGoalPosition
void initTrajectoryWaypoint(Manipulator actual_manipulator, Kinematics *kinematics)
initTrajectoryWaypoint
void error(STRING str)
error
JointWaypoint removeWaypointDynamicData(JointWaypoint value)
removeWaypointDynamicData
double getMoveTime()
getMoveTime
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToRPYVector
JointTrajectory getJointTrajectory()
getJointTrajectory
Eigen::VectorXd getCoefficient()
getCoefficient
void setManipulator(Manipulator manipulator)
setManipulator
void makeTaskTrajectory(TaskWaypoint start_way_point, TaskWaypoint goal_way_point)
makeTaskTrajectory
Eigen::Vector3d convertOmegaDotToRPYAcceleration(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
convertOmegaDotToRPYAcceleration
Name getPresentControlToolName()
getPresentControlToolName
Name getPresentCustomTrajectoryName()
getPresentCustomTrajectoryName
void setPresentTime(double present_time)
setPresentTime
Eigen::Matrix3d orientation
void makeJointTrajectory(JointWaypoint start_way_point, JointWaypoint goal_way_point)
makeJointTrajectory
double getTickTime()
getTickTime
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)
convertRPYToRotationMatrix
virtual ~JointTrajectory()
CustomJointTrajectory * getCustomJointTrajectory(Name name)
getCustomJointTrajectory
void setTrajectoryType(TrajectoryType trajectory_type)
setTrajectoryType
virtual ~TaskTrajectory()