Applying velocity constraints during Cartesian space planning

edit

Hi

I am using ROS-Indigo on Ubuntu 14.04 and using a 7 axis robot. I want to implement velocity constraints during Cartesian space planning but unable to do so as Moveit! by default doesnot implement velocity constraints for Cartesian Planning. I also tried to use Iterative Parabolic Time Parameterization to adjust the timestamps but no use. Is there any workaround to implement velocity control in Cartesian space ??

My code is as follows:

 #include <moveit/move_group_interface/move_group.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 #include <moveit_msgs/DisplayRobotState.h>
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <tf/transform_listener.h>

 int main(int argc, char **argv)

 {

   ros::init(argc, argv, "line");

   ros::NodeHandle node_handle;

   ros::AsyncSpinner spinner(1);

   spinner.start();

   moveit::planning_interface::MoveGroup group("arm");

   moveit::planning_interface::PlanningSceneInterface plannning_scene_interface ;

     tf::TransformListener listener;

   tf::StampedTransform transform;

   group.setGoalPositionTolerance(0.0001);

   group.setGoalOrientationTolerance(0.001);


   group.setPlannerId("RRTConnectkConfigDefault");

   std::vector<double> joint_positions;

   geometry_msgs::PoseStamped pose;

   group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), joint_positions);

   joint_positions[0] = -2.277;

   joint_positions[1] = -1.0;

   joint_positions[2] = 0.0;

   joint_positions[3] = -0.80;

   joint_positions[4] = 0.0;

   joint_positions[5] = -0.8;

   joint_positions[6] = 0;

   group.setJointValueTarget(joint_positions);

   std::vector<geometry_msgs::Pose> waypoints;

   group.move();

   sleep(2.0);

   listener.lookupTransform ("world", group.getEndEffectorLink().c_str(), ros::Time(0), transform );

  moveit::planning_interface::MoveGroup::Plan my_plan;

  pose.header.stamp = ros::Time::now();

  pose.header.frame_id = "/world";



  pose.pose.position.x = transform.getOrigin().getX();

  pose.pose.position.y = transform.getOrigin().getY();

  pose.pose.position.z = transform.getOrigin().getZ();



  pose.pose.orientation.x = transform.getRotation().getX();

  pose.pose.orientation.y = transform.getRotation().getY();

  pose.pose.orientation.z = transform.getRotation().getZ();

  pose.pose.orientation.w = transform.getRotation().getW();

 geometry_msgs::Pose pose2 = pose.pose;

   group.setMaxVelocityScalingFactor(0.50);

   group.setMaxAccelerationScalingFactor(0.50);


  pose2.position.x -= 0.90;

   waypoints.clear();
   waypoints.push_back(pose2);



   moveit_msgs::RobotTrajectory trajectory;

   const double jump_threshold = 0.0;

   const double eef_step = 0.001;

   double fraction = group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

   robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "arm");

   // Second get a RobotTrajectory from trajectory
   rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory);

   // Thrid create a IterativeParabolicTimeParameterization object
   trajectory_processing::IterativeParabolicTimeParameterization iptp;

   // Fourth compute computeTimeStamps
   bool success = iptp.computeTimeStamps(rt);
   ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

   // Get RobotTrajectory_msg from RobotTrajectory
   rt.getRobotTrajectoryMsg(trajectory);

   // Finally plan and execute the trajectory
   my_plan.trajectory_ = trajectory;
   ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);    
   sleep(5.0);
   group.execute(my_plan);
 sleep(2.0);

  ros::shutdown();

   return 0;

 }