Hello, I'm your lovely little fish. Recently, I feel that there are many problems related to moveit. After all, the application of robot arm + vision is really popular. Xiaoyu wants to teach robot arm motion planning directly.
Some students asked Xiaoyu, how to use moveit to control the real manipulator? Xiaoyu is very experienced in this. Xiaoyu used to use moveit to complete the motion planning of the real manipulator starting from the silidworks model. If you want to know how to do it, you can pay attention to the small fish. Xiaoyu is going to export urdf from solidworks model. Let's talk about robot modeling and Manipulator Simulation Based on moveit. Please pay close attention.
If you want to get the source code for the small fish test in this article, you can pay attention to the Gongzhong number and reply to moveit in the background.
1, A few questions before we start?
1. Why can't moveit directly control the real manipulator?
The reason is that there are too many types of real manipulator. Today you make a manipulator and tomorrow he will make a manipulator Moveit is only a framework for manipulator motion planning and is not responsible for driving the real manipulator.
2. What does moveit use to control the manipulator?
Since moveit cannot directly drive the manipulator, it must be controlled through a medium, which is our focus today - trajectory actuator.
2, How does Moveit control the of virtual manipulators in rviz and gazebo?
Careful students may have found that every time moveit starts, XX will be loaded_ Controller, if used when running moveit
rostopic list
You will see such a topic
/execute_trajectory/cancel/execute_trajectory/feedback/execute_trajectory/goal/execute_trajectory/result/execute_trajectory/status
If you use gazebo, the names may be different, but they all have the same prefix, followed by five names: cancel, feedback, goal, result and status.
This topic is the topic subscribed by the track execution service. Whoever the track 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 find out who the topic publisher and subscriber are respectively?
rostopic info /execute_trajectory/goalType: moveit_msgs/ExecuteTrajectoryActionGoalPublishers: * /rviz_monster_4900_3400225440813710105 (http://monster:33953/)Subscribers: * /move_group (http://monster:34519/)
You can try it with Xiaoyu. You are right. This topic is the connection between moveit and rviz. Good guy, here, the improper relationship between moveit and rviz has finally been discovered by the witty.
2. How does moveit control the manipulator in gazebo?
Many tutorials tell you that you should first demo Fake in launch_ Execution is fasle;
<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>
What does this sentence mean? In fact, it means telling rviz I don't want you to help me control the virtual manipulator in rviz. I already have someone outside and don't need you!
Then rviz will be sad. Even if you don't want me (execution), you always have to tell the person (Implementation), that is, tell rviz the joint angle of the current manipulator. After all, it needs to be original and execute the moveit command.
3. Who is in love with whom, gazebo and moveit?
So next, let's run gazebo to see what benefits gazebo gets from moveit, and then give something to moveit.
When true is changed to false, do not run gazebo, run moveit directly, and you will find that such a warning and ERROR will appear in the start loading instruction of 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 list
If you run gazebo and start moveit, the following INFO will appear
[ INFO] [1626355494.355057943, 322.100000000]: Added FollowJointTrajectory controller for [ INFO] [1626355494.355212191, 322.100000000]: Returned 1 controllers in list
Just like a tadpole looking for its mother, moveit will find the Action Server with the corresponding name as soon as it is started. Once it is found, it will be loaded and reserved for later calls.
What does gazebo offer?
! [data flow diagram] (01 how does moveit control a real manipulator?. assets/image-20210715221757439-1626358679296.png)
We use it again
rostopic list
If you check the topic list, you will find that there are more. This is actually the move of gazebo and moveit_ Group communication component.
rostopic list/arm_controller/follow_joint_trajectory/cancel/arm_controller/follow_joint_trajectory/feedback/arm_controller/follow_joint_trajectory/goal/arm_controller/follow_joint_trajectory/result/arm_controller/follow_joint_trajectory/status
More specifically, you can see from the figure below, rqt_graph is like this. gazebo subscription arm_controller's goal, publish joint_ The state topic realizes the simulation in this way.
! [rqt_graph] (01 how does moveit control a real manipulator?. assets/image-20210715213207249.png)
4. What does the track data sent by moveit to gazebo look like?
You can try this. It's very simple, because moveit will expose its request content through a topic, which is:
/move_group/display_planned_path
use
rostopic echo /move_group/display_planned_path
Then click plan and execute in rviz to see the print of this topic
model_id: "ur5"
trajectory:
-
joint_trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "world"
joint_names:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
points:
-
positions: [-0.0005475973198105777, 0.01301509545699897, -4.790678318222774e-05, 0.0006401223916085996, -1.1133751010916626e-05, 5.540891806798953e-05]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [4.964547491296066e-06, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 0
nsecs: 0
-
positions: [-0.0005122762372628266, 0.012990523657837615, -6.632561628428418e-05, 0.0006650964481281676, 2.001062241064062e-05, -0.6034930544710522]
velocities: [9.363578155606405e-06, -6.513955555019406e-06, -4.882811364940933e-06, 6.620593515710222e-06, 8.256337394129777e-06, -0.16000000000000003]
accelerations: [-3.5927592836451216e-21, 0.0, -8.981898209112804e-22, 0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 3
nsecs: 772177896
-
positions: [-0.0004769551547150755, 0.012965951858676258, -8.474444938634064e-05, 0.0006900705046477356, 5.1154995832197863e-05, -1.2070415178601723]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [-4.964547491296059e-06, 3.4536841762486802e-06, 2.588855297563277e-06, -3.5102233764803286e-06, -4.377490999290573e-06, 0.08483163011052242]
effort: []
time_from_start:
secs: 7
nsecs: 544355792
multi_dof_joint_trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
points: []
trajectory_start:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "world"
name:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
position: [-0.0005475973198105777, 0.01301509545699897, -4.790678318222774e-05, 0.0006401223916085996, -1.1133751010916626e-05, 5.540891806798953e-05]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
multi_dof_joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "world"
joint_names: []
transforms: []
twist: []
wrench: []
attached_collision_objects: []
is_diff: False
---This data is actually the trajectory points of the manipulator joint space varying with time, and contains time, velocity and acceleration information. Since gazebo can be used, how should this point be used? Please read on:
3, Get the data and send it to the real manipulator
The above describes how to use moveit to control the virtual manipulator, and also knows what the data of moveit finally controls the manipulator looks like.
It's easy for us to control the real manipulator. We just need to start an action like gazebo, get the trajectory data and feed back the pose of the real manipulator in real time.
Then we start step by step.
1. The first step is to get the output of moveit - trajectory data
Customize the Action Server to get the data
As mentioned above, moveit will automatically load the actionServer with the corresponding name when it is started, so if we write an actionServer ourselves and the name remains the same as that required by moveit, will moveit treat us as actuators.
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_start=False)After completing the above code, we can actually receive the trajectory data of the corresponding controller.
2. The second step is to feed back the joint data of the manipulator in real time
In fact, this is also very simple. No matter what brand the manipulator is, as long as it can be connected to our computer through serial port / network, we can use the SDK of the corresponding manufacturer to read the joint angle data of the manipulator.
After reading the data, pass it through / joint_ Just publish the state topic.
In fact, we do this by imitating gazebo, inputting trajectory data and outputting real-time joint data of real manipulator.
3. Send to real manipulator
This step is also the most important one, because it may involve different functions of different manipulators from different manufacturers.
Before you want to control the real manipulator, you need to determine what data the real manipulator needs. Can the trajectory data we get from moveit be directly sent to six joints for execution? The answer must be yes. But you will find that the final effect of robot execution may be different from what you think (because of trajectory interpolation). Here's a summary.
We can divide our mechanical arms into three categories:
3.1 category I: Industrial arm / cooperative arm from large factories
This kind of manipulator generally provides trajectory interpolation algorithm, which can directly say that the information generated by moveit about the change of joint angle with time is sent to it through the control interface of this kind of manipulator. This kind of manipulator will automatically interpolate and then execute.
For example, the robot arm of AUBO can execute the trajectory through the start point, path point and end point.
3.2 type II: mechanical arm for education from small factory
This kind of manipulator also has a corresponding operation SDK. You only need to read it carefully to determine whether it supports the launch trajectory for execution. If not, go another way to determine the control cycle of its motor, and interpolate the joint space attitude data generated by moveit by cubic or quintic terms. If you don't know how to do it, You can focus on the official account of WeChat's public fish and help them with small fish. Later, the fish will also write how to interpolate the joint and DeCarr space.
3.3 category III: own diy manipulator
Small fish of this kind of manipulator has done it in college. If real-time performance is not considered, it is actually relatively simple. Generally, it is OK to directly write embedded programs to drive servo motors.
Xiaoyu used STM32 to drive the steering gear in college. It was probably in the winter vacation of freshman year. Now it's far away, but it's very interesting.
For this kind of students, Xiaoyu's suggestion is to directly connect to the computer through the serial port, write a node in python on the computer for serial port communication, and send instructions to the motor. At the same time, this node receives the control data of moveit, performs trajectory interpolation, and then executes it. At the same time, it should also release the angle of each motor.
4, To sum up
I have written thousands of words and feel dizzy. If it is helpful to you, please pay attention to Xiaoyu. Your attention is the greatest encouragement to Xiaoyu.
Finally, I would like to recommend another wave. If you want to further study moveit, path planning, collision detection, three-dimensional environment perception, ROS2 and other robot technologies, you might as well pay attention to Xiaoyu's wechat Gongzhong Yuxiang ROS
Author introduction
I am Xiaoyu, a senior player in the field of robotics, and now I am an algorithm engineer of a one legged robot in Shenzhen
Learning programming in junior high school, learning robotics in senior high school, playing robot related competitions in college to achieve a monthly income of 2W + (competition bonus)
At present, we are doing official account, exporting robot learning guide, notes and working experience. Welcome everyone to pay attention to small fish, exchange technology and learn robots together.
