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 > #include > #include > #include > #include > #include >>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 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;>> }
↧