Changing Orientation of manipulator arm in steps

asked 2018-07-04 01:36:39 -0500

Pulkit123 gravatar image

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;
> 
> }
edit retag flag offensive close merge delete

Comments

Are you sure the end effector is within the dexterous workspace of the robot? I.e. it is possible for its end effector top be at any angle. It's also perfectly possible for one of these plans to move the end effector as well as rotate it.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-07-04 02:00:03 -0500 )edit