!
| Note: This tutorial assumes basic familiarity with the OUR robot and MoveGroup class.Otherwise,you should have already finished learnning the front tutorials.. This tutorial assumes that you have completed the previous tutorials: Plan a trajectory to control the OUR robot motion in the Rviz. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Plan a trajectory to control an OUR robot motion in the real world.
Description: Use the MoveGroup class for planning trajectories to control an OUR robot motion the real world.Keywords: MoveGroup trajectory plan OUR
Tutorial Level: BEGINNER
Contents
Pre-requisites
1. Already generate the necessary configure file for OUR the robot using MoveIt! setup assistant.
2. Have the basic driver package for OUR robot.
3. You have already installed the USB-CAN driver.
Use the Move Group Interface to plan a trajectory sending to the topic of "moveJ"
We use the MoveGroup class to set joint or pose goal,creating motion plans, decomposing plans and sending the positions message to the topic of "moveJ".
(1) The MoveGroup class can be easily setup using just the name of the group you would like to control and plan for.
1 moveit::planning_interface::MoveGroup group("joint_group");
(2) Getting Basic Information
(3) First get the current set of joint values for the group.
- let’s modify one of the joints, plan to the new joint space goal. we call the planner to compute the plan.
1 std::vector<double> group_variable_values;
2 group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
3
4 group_variable_values[0] = 0;
5 group_variable_values[1] = 20*M_PI/180.0;
6 group_variable_values[2] = -70*M_PI/180.0;
7 group_variable_values[3] = 0;
8 group_variable_values[4] = -90*M_PI/180.0;
9 group_variable_values[5] = 0;
10 group.setJointValueTarget(group_variable_values);
11 moveit::planning_interface::MoveGroup::Plan my_plan;
12 bool success = group.plan(my_plan);
(4) Discompose the trajectory and publish to the topic named "moveJ"
1 std_msgs::Float32MultiArray joints;
2 joints.data.resize(6);
3 ros::NodeHandle nh;
4 ros::Publisher command_pub = nh.advertise<std_msgs::Float32MultiArray> ("moveJ", 1000);
5 ros::Rate loop_rate(20);//Hz
6 int j =0;
7 while(ros::ok())
8 {
9 if( j != my_plan.trajectory_.joint_trajectory.points.size()) //or return and again
10 {
11 for(j=0; j<my_plan.trajectory_.joint_trajectory.points.size(); j++)
12 {
13 for(int i=0; i<6; i++)
14 {
15 joints.data[i]=my_plan.trajectory_.joint_trajectory.points[j].positions[i];
16 ROS_INFO("send ponits[%d]positions[%d] %f",j,i,joints.data[i]);
17 }
18 command_pub.publish(joints);
19 loop_rate.sleep();
20 }
21 }
22 }
Write the node to subscribe the "moveJ" topic and control the real robot
(1) Subscribe the "moveJ" topic
(2) The callback function is used to send the position message to real robot.
1 JointControlCanBus *jointContorlHandler = new JointControlCanBus();
2 void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
3 {
4 ROS_INFO("[%f,%f,%f,%f,%f,%f]",
5 msg->data[0],msg->data[1],msg->data[2],msg->data[3],msg->data[4],msg->data[5]);
6
7
8 jointContorlHandler->setTagPosRadio(0x01, msg->data[0], MODEL_TYPE_J80, CMDTYPE_WR_NR);
9
10 jointContorlHandler->setTagPosRadio(0x02, msg->data[1], MODEL_TYPE_J80, CMDTYPE_WR_NR);
11 jointContorlHandler->setTagPosRadio(0x03, msg->data[2], MODEL_TYPE_J80, CMDTYPE_WR_NR);
12 jointContorlHandler->setTagPosRadio(0x04, msg->data[3], MODEL_TYPE_J60, CMDTYPE_WR_NR);
13 jointContorlHandler->setTagPosRadio(0x05, msg->data[4], MODEL_TYPE_J60, CMDTYPE_WR_NR);
14 jointContorlHandler->setTagPosRadio(0x06, msg->data[5], MODEL_TYPE_J60, CMDTYPE_WR_NR);
15 }
Running the cod
- You can also organize the following command in a launch file. (1)roslaunch our_moveit_config demo.launch (2)rosrun our_basic joint_control (3)rosrun our_robot kdl_rviz
