#include <robotis_manipulator_trajectory_generator.h>

Collaboration diagram for robotis_manipulator::TaskTrajectory:

Public Member Functions
	TaskTrajectory ()

virtual	~TaskTrajectory ()

void	makeTaskTrajectory (double move_time, TaskWaypoint start, TaskWaypoint goal)
	makeTaskTrajectory More...

Eigen::MatrixXd	getMinimumJerkCoefficient ()
	getMinimumJerkCoefficient More...

TaskWaypoint	getTaskWaypoint (double tick)
	getTaskWaypoint More...

Private Attributes
uint8_t	coefficient_size_

MinimumJerk	minimum_jerk_trajectory_generator_

Eigen::MatrixXd	minimum_jerk_coefficient_

Detailed Description

The TaskTrajectory class.

Definition at line 116 of file robotis_manipulator_trajectory_generator.h.

Constructor & Destructor Documentation

TaskTrajectory::TaskTrajectory ( )

Definition at line 132 of file robotis_manipulator_trajectory_generator.cpp.

 {
   minimum_jerk_coefficient_ = Eigen::MatrixXd::Identity(6, 4);
 }

TaskTrajectory::~TaskTrajectory ( )

virtual

Definition at line 136 of file robotis_manipulator_trajectory_generator.cpp.

136 {}

Member Function Documentation

Eigen::MatrixXd TaskTrajectory::getMinimumJerkCoefficient ( )

getMinimumJerkCoefficient

Returns

Definition at line 267 of file robotis_manipulator_trajectory_generator.cpp.

 {
   return minimum_jerk_coefficient_;
 }

TaskWaypoint TaskTrajectory::getTaskWaypoint ( double tick )

getTaskWaypoint

Parameters

tick

Returns

Definition at line 209 of file robotis_manipulator_trajectory_generator.cpp.

 {
   std::vector<Point> result_point;
   for (uint8_t index = 0; index < coefficient_size_; index++)
   {
     Point single_task_way_point;
 
     single_task_way_point.position = minimum_jerk_coefficient_(0, index) +
              minimum_jerk_coefficient_(1, index) * pow(tick, 1) +
              minimum_jerk_coefficient_(2, index) * pow(tick, 2) +
              minimum_jerk_coefficient_(3, index) * pow(tick, 3) +
              minimum_jerk_coefficient_(4, index) * pow(tick, 4) +
              minimum_jerk_coefficient_(5, index) * pow(tick, 5);
 
     single_task_way_point.velocity = minimum_jerk_coefficient_(1, index) +
              2 * minimum_jerk_coefficient_(2, index) * pow(tick, 1) +
              3 * minimum_jerk_coefficient_(3, index) * pow(tick, 2) +
              4 * minimum_jerk_coefficient_(4, index) * pow(tick, 3) +
              5 * minimum_jerk_coefficient_(5, index) * pow(tick, 4);
 
     single_task_way_point.acceleration = 2 * minimum_jerk_coefficient_(2, index) +
              6 * minimum_jerk_coefficient_(3, index) * pow(tick, 1) +
              12 * minimum_jerk_coefficient_(4, index) * pow(tick, 2) +
              20 * minimum_jerk_coefficient_(5, index) * pow(tick, 3);
 
     single_task_way_point.effort = 0.0;
 
     result_point.push_back(single_task_way_point);
   }
 
   TaskWaypoint task_way_point;
   for(uint8_t i = 0; i < 3; i++)        //x ,y ,z
   {
     task_way_point.kinematic.position[i] = result_point.at(i).position;
     task_way_point.dynamic.linear.velocity[i] = result_point.at(i).velocity;
     task_way_point.dynamic.linear.acceleration[i] = result_point.at(i).acceleration;
   }
 
   Eigen::Vector3d rpy_orientation;
   rpy_orientation << result_point.at(3).position, result_point.at(4).position, result_point.at(5).position;
   task_way_point.kinematic.orientation = math::convertRPYToRotationMatrix(result_point.at(3).position,   //roll
                                                                        result_point.at(4).position,   //pitch
                                                                        result_point.at(5).position);   //yaw
 
   Eigen::Vector3d rpy_velocity;
   rpy_velocity << result_point.at(3).velocity, result_point.at(4).velocity, result_point.at(5).velocity;
   task_way_point.dynamic.angular.velocity = math::convertRPYVelocityToOmega(rpy_orientation, rpy_velocity);
 
   Eigen::Vector3d rpy_acceleration;
   rpy_acceleration << result_point.at(3).acceleration, result_point.at(4).acceleration, result_point.at(5).acceleration;
   task_way_point.dynamic.angular.acceleration = math::convertRPYAccelerationToOmegaDot(rpy_orientation, rpy_velocity, rpy_acceleration);
 
   return task_way_point;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void TaskTrajectory::makeTaskTrajectory	(	double	move_time,
		TaskWaypoint	start,
		TaskWaypoint	goal
	)

makeTaskTrajectory

Parameters

move_time
start
goal

Definition at line 138 of file robotis_manipulator_trajectory_generator.cpp.

 {
   std::vector<Point> start_way_point;
   std::vector<Point> goal_way_point;
 
   for(uint8_t i = 0; i < 3; i++)      //x, y, z
   {
     Point position_temp;
     position_temp.position = start.kinematic.position[i];
     position_temp.velocity = start.dynamic.linear.velocity[i];
     position_temp.acceleration = start.dynamic.linear.acceleration[i];
     position_temp.effort = 0.0;
     start_way_point.push_back(position_temp);
 
     position_temp.position = goal.kinematic.position[i];
     position_temp.velocity = goal.dynamic.linear.velocity[i];
     position_temp.acceleration = goal.dynamic.linear.acceleration[i];
     position_temp.effort = 0.0;
     goal_way_point.push_back(position_temp);
   }
 
 
   Eigen::Vector3d start_orientation_rpy;
   Eigen::Vector3d start_ang_vel_rpy;
   Eigen::Vector3d start_ang_acc_rpy;
 
   start_orientation_rpy = math::convertRotationMatrixToRPYVector(start.kinematic.orientation);
   start_ang_vel_rpy = math::convertOmegaToRPYVelocity(start_orientation_rpy, start.dynamic.angular.velocity);
   start_ang_acc_rpy = math::convertOmegaDotToRPYAcceleration(start_orientation_rpy, start_ang_vel_rpy, start.dynamic.angular.acceleration);
 
   Eigen::Vector3d goal_orientation_rpy;
   Eigen::Vector3d goal_ang_vel_rpy;
   Eigen::Vector3d goal_ang_acc_rpy;
 
   goal_orientation_rpy = math::convertRotationMatrixToRPYVector(goal.kinematic.orientation);
   goal_ang_vel_rpy = math::convertOmegaToRPYVelocity(goal_orientation_rpy, goal.dynamic.angular.velocity);
   start_ang_acc_rpy = math::convertOmegaDotToRPYAcceleration(goal_orientation_rpy, goal_ang_vel_rpy, goal.dynamic.angular.acceleration);
 
   for(uint8_t i = 0; i < 3; i++)    //roll, pitch, yaw
   {
     Point orientation_temp;
     orientation_temp.position = start_orientation_rpy[i];
     orientation_temp.velocity = start_ang_vel_rpy[i];
     orientation_temp.acceleration = start_ang_acc_rpy[i];
     orientation_temp.effort = 0.0;
     start_way_point.push_back(orientation_temp);
 
     orientation_temp.position = goal_orientation_rpy[i];
     orientation_temp.velocity = goal_ang_vel_rpy[i];
     orientation_temp.acceleration = goal_ang_acc_rpy[i];
     orientation_temp.effort = 0.0;
     goal_way_point.push_back(orientation_temp);
   }
 
   coefficient_size_ = start_way_point.size();
   minimum_jerk_coefficient_.resize(6,coefficient_size_);
   for (uint8_t index = 0; index < coefficient_size_; index++)
   {
     minimum_jerk_trajectory_generator_.calcCoefficient(start_way_point.at(index),
                                     goal_way_point.at(index),
                                     move_time);
 
     minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
   }
 }