Changing Orientation of manipulator arm in steps

edit

Hi

I am working on a 6 DOF manipulator arm with ROS Indigo and Ubuntu 14.04. I have been trying to change the end-effector's orientation (roll , pitch, yaw) in steps to see the continuous change in end-effector orientation values, but the robot often distracts fro the path and follows another trajectory. I tried to increase the planning time but still no use. CAn anyone explain how to achieve this ?? Below is my code :

> #include <moveit/move_group_interface/move_group.h>
> #include <moveit/planning_scene_interface/planning_scene_interface.h>
> #include <moveit_msgs/RobotTrajectory.h>
> #include <tf/transform_listener.h>
> #include <tf/tf.h>
> #include <tf/transform_datatypes.h>
> 
>int main(int argc, char **argv) {  
>ros::init(argc, argv,"tool");  
>ros::NodeHandle nh;  
>ros::AsyncSpinner spinner(2);  
>spinner.start();
> 
>moveit::planning_interface::MoveGroup group("arm");  
>moveit::planning_interface::PlanningSceneInterface ps;  
>moveit::planning_interface::MoveGroup::Plan my_plan;
> 
>group.setGoalTolerance(0.0001);  
>group.setGoalOrientationTolerance(0.001);
>group.setPlannerId("RRTConnectkConfigDefault");
> 
> 
>tf::TransformListener listener;
> 
>tf::StampedTransform transform;
> 
> std::vector<double> joint_positions;  
> joint_positions.resize(6);
> joint_positions[0] = -1.112672;   
  joint_positions[1] = 0.543184;  
> joint_positions[2] = 0.149478;  
> joint_positions[3] = -1.548138;  
> joint_positions[4] = -3.057866;  
> joint_positions[5] = -1.020237;  
> joint_positions[6] = -2.201550;
> 
>  
>group.setJointValueTarget(joint_positions);
>group.move();
>sleep(5.0); 
 geometry_msgs::PoseStamped pose;
>pose.header.stamp = ros::Time::now();
> 
>listener.lookupTransform ("world",
>group.getEndEffectorLink().c_str(),
>ros::Time(0), transform );
> 
>pose.header.frame_id = "/world";
> // Getting Quaternion values
>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();
> 
> // Quaternion to roll, pitch , yaw
>tf::Quaternion q(pose.pose.orientation.x,pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w );  
>tf::Matrix3x3 m(q);
>double r, p, y, step;    
  m.getRPY(r, p, y);
  step = 0.1;   
   for(int i =1; i<= 50; i++)   {  
>      r = r + 0.01;     // changing the roll value
>      pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r,p,y);  // updating the quaternion
>      group.setPoseTarget(pose,"end_eff_link");   // giving the updated goal moveit
>      group.setPlanningTime(500.0);
>      group.move();
>       //group.plan(my_plan);
>    }
> 
> 
>   ros::shutdown();
> 
>   return 0;
> 
> }