Hi, everybody. I’m your cute little wisdom. Recently, there are a lot of problems related to moveIt. After all, the application of mechanical arm + vision is really very popular. Xiao Zhi wants to teach the movement planning of mechanical arm directly.

Some students asked Xiaozhi, how can I use moveIt to control the real robot arm? Xiao Zhi is very experienced in this. He used to start from the SilidWorkDS model and use MoveIt to complete the motion planning of the real mechanical arm. To learn how to do this, follow Satoshi. Xiaozhi is going to start from solidWorks model to export URDF, and explain moveIt based robot modeling, mechanical arm simulation, etc., please continue to pay attention to.

If you want to obtain the source code of the small wisdom test in this article, you can pay attention to the work number, the background reply moveIt can be.

How many questions do you need to know before you start?

1. Why can’t MoveIt directly control the real robot arm?

The reason is that there are so many different types of real arms, one day you make a robot arm, the next day he makes a robot arm. Moveit is a framework for planning the movement of the manipulator, not for driving the actual manipulator.

2. How does Moveit control the robot arm?

Moveit cannot drive the manipulator directly, so it must be controlled through a medium, and that medium will be our focus today — the trajectory actuator.

How does Moveit control rVIZ and Gazebo?

If you’re careful, you might have noticed that moveIt loads xx_controller every time it starts, if you use it when you run moveIt

rostopic list
Copy the code

You will see that there is a topic like this

/execute_trajectory/cancel/
/execute_trajectory/feedback/
/execute_trajectory/goal/execute_trajectory/result/
/execute_trajectory/status
Copy the code

If gazebo is used, it may have a different name, but they all have the same prefix, followed by cancel, feedback, goal, result and status.

This topic is the topic to which the trajectory execution service subscribles, and whoever the trajectory executor is will provide these five topic services (in fact, this is not an ordinary topic, but the Action communication mechanism in ROS).

1. We can use the following command to check, who are the topic publisher and subscriber respectively?

rostopic info /execute_trajectory/goal
Type: moveit_msgs/ExecuteTrajectoryActionGoal
Publishers:  * /rviz_monster_4900_3400225440813710105 (http://monster:33953/)
Subscribers:  * /move_group (http://monster:34519/)
Copy the code

This topic is the link between MOVEIt and RVIZ. Boy, here we go. The improper relationship between MoveIt and RVIZ has finally been spotted by our wits.

2. How does MoveIt control the gazebo manipulator?

Many tutorials tell you to start with fake_execution as fasle in demo.launch;

  <include file="$(find ur5_moveit_config)/launch/move_group.launch">    
  <arg name="allow_trajectory_execution" value="true"/>     
  <arg name="fake_execution" value="true"/>    
  <arg name="info" value="true"/>    
  <arg name="debug" value="$(arg debug)"/>  </include>
Copy the code

I don’t want you to help me control the virtual robot arm in RVIZ. I already have someone on the outside. I don’t need you.

The rviz sad, even if you don’t I (execute), the person that you always want to tell (gazebo) (execute), is also tell rviz current mechanical arm joint Angle, and original after all, to execute commands moveit.

3. Who loves Who, Gazebo or MoveIt?

So let’s run Gazebo, and see what gazebo takes from MoveIt, and what gazebo gives to MoveIt.

Moveit (gazebo, moveIt, moveIt, gazebo, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt, moveIt

[WARN] [1626355641.007776896, 8.232000000] : Waiting for/FOLLOW_joint_trajectory to come up [WARN] [1626355647.034715552, 14.232000000]: Waiting for/FOLLOW_joint_trajectory to come up [ERROR] [1626355653.060493182, 20.233000000]: Action client Not connected: / FOLLOW_JOINt_trajectory [INFO] [1626355653.085575856, 20.258000000]: Returned 0 controllers in listCopy the code

If you run Gazebo and then launch MoveIt it becomes INFO below.

[the INFO] [1626355494.355057943, 322.100000000] : Added FollowJointTrajectory Controller for [INFO] [1626355494.355212191, 322.100000000]: Returned 1 controllers in listCopy the code

Like a tadpole looking for its mother, moveIt starts looking for an Action Server with its name, and once it finds it loads it for later call.

And what does Gazebo offer?

Again we use

rostopic list
Copy the code

If you look at the list of topics, you’ll see that this is the communication component between Gazebo and Move_Group of MoveIt.

rostopiclist/arm_controller/follow_joint_trajectory/cancel/arm_controller/follow_joint_trajectory/feedback/arm_controlle r/follow_joint_trajectory/goal/arm_controller/follow_joint_trajectory/result/arm_controller/follow_joint_trajectory/stat usCopy the code

And if you look at the graph a little bit more concretely, rqt_graph looks something like this. Gazebo subscribes to the arm_controller goal and publishes the JoinT_state topic.

4. What does moveIt send gazebo trajectory data look like?

This is something you can try, and it’s very simple, because moveIt will expose its request via a topic, which is:

/move_group/display_planned_path
Copy the code

use

rostopic echo /move_group/display_planned_path
Copy the code

Then click on plan and Execute randomly in RVIZ to see the topic printed

Copy the code

This data is actually the trajectory point of the manipulator’s joint space changing with time, and contains time, speed and acceleration information. So if gazebo is using this, how do you use this point? Please continue to read:

Three, get the data and send it to the real robot arm

We’ve covered how to use moveIt to control the virtual manipulator, and we’ve also seen what the data for moveIt’s final manipulator will look like.

If we want to control the real robot arm, it is very simple. We just need to start an action like Gazebo, get the trajectory data and feed back the real robot arm’s position in real time.

So let’s start with one step at a time.

1. The first step is to get the moveIt output — trajectory data

Customize the Action’s Server to get the data

If we write an actionServer with the same name as the one that moveit wants, then moveit will treat us as the executor.

We use Python to implement:

def on_goal(goal_handle): global goal goal_handle.set_accepted() goal = goal_handle.get_goal() goal = goal.trajectory.points goal_handle.set_succeeded() def on_cancel(goal_handle): goal_handle.set_aborted() goal_handle.set_canceled() rospy.init_node('calculate_py', disable_signals=True) server = actionlib.ActionServer("jaka_controller/follow_joint_trajectory",FollowJointTrajectoryAction,on_goal,on_cancel,auto_star t=False)Copy the code

By completing the code above, we can actually receive the trajectory data for the controller.

2. The second step is real-time feedback of the joint data of the manipulator

This point is actually very simple. No matter what brand your manipulator is, as long as it can be connected to our computer through serial port/network, we can read the joint Angle data of the manipulator by using the SDK of the corresponding manufacturer.

Once the data is read, publish it using the/joinT_state topic.

What we’re doing here is essentially imitating Gazebo, putting in trajectory data, and putting out real-time joint data of the real robot arm.

3. Send real mechanical arms

This step is also the most important one, because it may involve different functions of different robotic arms from different manufacturers.

Before we want to control the real manipulator, we need to determine what data the real manipulator needs, can we send the trajectory data from moveIt directly to the six joints for execution? The answer is definitely yes. However, you may find that the result of the robot implementation may not be quite as you expected (due to the trajectory interpolation reasons), here is a summary of Satoshi.

You can divide your arms into three categories:

3.1 The first category: industrial arm/cooperative arm from dACHang

This type of manipulator typically provides trajectory interpolation algorithms. You can directly tell moveIt to generate information about joint angles over time through the control interface of the manipulator. The manipulator will automatically interpolate and then perform the interpolation.

AUBO’s robotic arm, for example, can execute a trajectory through starting points and waypoints and end points.

3.2 The second category: mechanical arms from small factories for education

Such mechanical arm also have the corresponding operation SDK, only need to carefully read it carefully, to determine whether it supports hair path of execution, if not, then go the other way, determine its motor control cycle, for moveit attitude data generated by the joint space for three or five items of interpolation, if you don’t know what to do, You can follow Xiaozhi’s wechat official account to contact xiaozhi for help. Xiaozhi will also write how to carry out the trajectory interpolation algorithm of joint and Decarl space.

3.3 Category 3: Diy robotic arms

This kind of mechanical arm was made by Xiaozhi when he was in college. If real-time performance is not considered, it is relatively simple. Generally, the servo motor can be driven directly by writing embedded programs.

Xiaozhi used STM32 steering gear in university, probably in the winter vacation of freshman year, now imagine that it is very far away, but also quite interesting.

For this kind of students, xiaozhi suggested that they directly connect to the computer through the serial port, write a node on the computer to communicate with the serial port, and send instructions to the motor. At the same time, this node receives the control data of MOVEIT, interpolates the trajectory and then executes, and also publishes the Angle of each motor.

Four, to sum up

I wrote thousands of words, dizzy, if it is helpful to you, please pay attention to small wisdom, your attention is the biggest encouragement to small wisdom.

Finally, if you want to further learn moveIt, path planning, collision detection, THREE-DIMENSIONAL environment perception, ROS2 and other robot technologies, you may wish to pay attention to Xiaozhi’s wechat Gongzhong Smart people.

The authors introduce

I am Xiaozhi, a senior player in the field of robotics, and an algorithm engineer of a one-legged robot in Shenzhen

I learned programming in junior high school and began to learn robotics in senior high school. During college, I achieved a monthly income of 2W+ in robot-related competitions (prize money).

At present, I am doing a public account, output robot learning guide, paper notes, work experience, welcome everyone to pay attention to xiaozhi, exchange technology together, learn robot.