move_group joint space goal
Hi,
I am using MoveGroupInterface
of MoveIt
to do basic motion planning of a UR5 robotic arm. For planning to a joint-space goal, I create a publisher for visualizing plans in rviz
.
display_pub = nh.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
I specify a vector of joint values, and set as the target.
double refVals [] = {-1.8963, -0.5800, -2.2339, -1.8976, 1.5629, 2.0322};
std::vector<double> newJointVals;
for (int i=0; i<6; i++) {
newJointVals.push_back(refVals[i]);
}
move_group.setJointValueTarget(newJointVals);
And, then, I make planning to this joint-space goal as follows:
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
where, my_plan has been declared as,
moveit::planning_interface::MoveGroupInterface::Plan my_plan
After this, if I run the following sequence of commands, I should be able to see the motion plans within rviz.
display_trajectory.trajectory_start = my_plan.start_state_;
display_trajectory.trajectory.push_back(my_plan.trajectory_);
display_pub.publish(display_trajectory)
But, I do not see any motion, when I launch the program. However, If I do the following instead,
move_group.execute(my_plan);
I can see the robotic arm moving to the desired joint-space goal. Please, can somebody help with this.
thanks,