Torque control of a Throwing Trajectory by a WAM
Abstract
In this project, a dynamic simulator is written to simulate the dynamic model of the four-degree of freedom WAM. The purpose of this simulation is to find a trajectory of the arm that can achieve an end effector’s speed that is more than 6 m/s for a throwing problem. I use a PID controller based on velocity control to get the trajectory. And the trajectory is tested on the WAM.
Introduction
WAM is a 7-dof robotic arm manipulator. Simulating the 7-dof-motion is very hard. But as in the throwing problem, the motion above the wrist doesn’t contribute too much to the speed of the ball. Therefore a 4-dof-arm simulator is enough for a throwing problem to find the end effector’s speed.
In this report, I first explain how I build the dynamic model of the simulation. Then the motion strategy that I use for this throwing problem and a PID controller based on angular speed is introduced. Finally, the results from the simulation are shown and a comparison between the experimental result and the simulation result is given.
1. Related work
In 2008, the Ishikawa-Namiki-Komoro lab in Tokyo University used a 3-dof WAM to do a high speed-throwing project. In that project, they use joint 1, joint 3 and joint 4 of the WAM to finish a throw. The motion is an analogous swing motion in which joint 3 kept the forearm in the horizontal position and joint 2 was fixed in a certain position.
In this experiment, the maximum velocity of the end effector is 6m/s.
2. Dynamic Simulation
The dynamic simulation uses the Lagrange equations to get the angular acceleration from the torque of each joint.
First, I computed the body Jacobian of each joint Ji corresponding to Mi, where Mi is the ith joint’s inertia matrix. Therefore the manipulator inertia. Therefore the manipulator inertia matrix M (θ) can be calculated as
. Also calculate potential part,
.Second, I computed the torques of each joint using Lagrange equation, which is,
. where τ represents the torque of the joint and,
.After expanding the components of the equation, the equation is as following:
. where C(θ,θ ̇ ) is the Coriolis and centrifugal force term and N(θ,θ ̇ ) is the gravity term.
3. PID Controller to Get the Trajectory
First, I use an angular speed simulator that assumes all the joints reach the desired speed instantaneously. In the simulation, the end effector reaches the maximum speed when all the four joints are moving at their maximum angular speed from a curved position to a line position. In the simulation, I tested both 2D and 3D motion and find that 3D motion can get a bigger end effector’s speed. The explanation for this is as follows:
. In this equation, we can see that the angular velocity will affect the speed of the chain. Take the upper arm in this project for example. If the arm moves in a 2D plane, only joint1 or joint2 will move. As the rotation arises of joint 1 and joint 2 are both perpendicular to the upper arm. So when joint 1 and joint 2 move at the same time, the angular velocity equals to the sum of the angular velocity of joint 1 and the angular velocity of joint 2. The sum of the two velocities is also perpendicular to the upper arm but is bigger than either of the velocities. So using the equation above, we can get a bigger speed of the upper arm in a 3D motion. It is the same with the forearm.
Therefore, using the four joints at the same time and making the two arms moving in the same time wise will add the four angular velocity together to get a large speed of the end effector. In this way, the arm can archive a far better end effector speed than arm moves in a 2D plane. This has also been seen in the velocity control simulation. The max speeds of the four joints are:
• V1 = 2.5 rad/s
• V2 = 3.7 rad/s
• V3 = 6.2 rad/s
• V4 = 5.8 rad/s
Therefore, in order to get a line position of the arm, I set the initial position of the arm as:
• Joint 1: 50 degree
• Joint 2: - 74 degree
• Joint 3: 64 degree (starts to move in the middle of the motion)
• Joint4: 116 degree
In this way the arm can move at the maximum speed and get into a line in the final position which get the maximum distance between the end effector and the base, thus get the best speed of the end effector.
Based on the assumption above, a torque PID controller based on angular velocity is written. In this controller, the torque is computed in the way as follows:
. where error = desired speed – current speed.
After get the torques from the above function, I put the torques into the dynamic equation which has been discussed in chapter 2 and calculate the accelerations of the joints. Then I get the new speeds of the joints.
The coefficients of the PID controller are:
• P coefficient = 20
• I coefficient = 0.1
• D coefficient = 30
4. Result and Explanation
4.1 Angular Speeds of the Joints
. Figure 1 The angular speeds of the 4 joints
In the results of the simulation, the maximum angular speeds are all achievable during the motion of the arm. (Joint 3 is slow because it is restricted to be still in the first 0.4 seconds). This also indicates that the speed in the angular speed control simulation is achievable.
4.2 The Motion of the Arm And the Speed of the End Effector
The following is the contrast diagram of the PID motion and the ideal motion which all the joints reach the max speed instantaneously.
. Figure 2 The motion of PID controller and the ideal motion
In fig 2, the up right is the motion of the dynamic simulation and the down left is the ideal motion. The red part is the trajectory space of the upper arm and the blue part is the trajectory space of the lower arm. The up left and the down right diagrams are the end effector’s speed diagram of the dynamic simulation and ideal motion.
As can be seen from the diagram, the speed of the end effector increases during the motion and reaches the maximum at the final position. If all the joints are moving at their maximum angular speed, the maximum speed of the end effector is 6.5m/s. Also, we can see that the motion of the dynamic model is not stable at the beginning of the motion and becomes smooth after the first 0.1 seconds.
4.3 The torques
The diagrams below are the torques of the 4 joints in the dynamic simulation.
. Figure 3 The torques of the four joints
As can be seen above, the torques vibrates a lot in the beginning of the motion and get steady after the first 0.15 seconds. The torques increases sharply in a very short time at the beginning of the motion. This corresponds to the sharp curve of the end effector’s speed at the beginning.
4.4 Comparison Between Position Control and Velocity Control
A position PID controller and a velocity PID controller are both used in the dynamic simulation.
The motions of the two controllers are almost the same. But in the position controller the torques and angular speed vibrate a lot. This is because in position control, the arm must wait until all the joints reach the desired position. Although in position control can guarantee the arm moves in the desired trajectory, in a throwing problem it is more important to keep the motion smooth. Therefore under this situation, it is better to use velocity PID controller.
4.5 Comparison Between the Results of the WAM and the Dynamic Model
To verify the dynamic simulation, a comparison between the experimental result and the dynamic simulation is made. The following are the diagrams that show results from the motion of WAM and the dynamic simulation. (The experimental result starts from 12 on the horizontal axis)
. Figure 4 The torque output of joint2 in the experiment result
. Figure 5 The torques output of the dynamic simulation
. Figure 6 the position output of each joint in the dynamic simulation
As can be seen in fig 6, the positions of the joints vibrate during the accelerating process. The trajectory is completed in 2 seconds. This position history is similar to the output of the experimental result but the experimental result takes a relatively longer time to finish the trajectory. Also in fig 4 and fig 5, we can see that the output of the torques of joint 2 has a similar shape between the simulation and the experimental result. However, the torques of the simulation is also higher that the experimental result. All these is because that I use a much bigger coefficient in the simulation PD controller to maintain a continuous motion.
The position controller that is used is as follows:
. where error = desired position – current position.
The WAM uses a PD controller with the coefficients:
| Joints | P coefficient | D coefficient |
| Joint 1 | 180 | 14 |
| Joint 2 | 140 | 10 |
| Joint 3 | 70 | 0 |
| Joint 4 | 80.2 | 3 |
In the simulation, a same PD controller is used but with much bigger coefficients to keep the trajectory:
| Joints | P coefficient | D coefficient |
| Joint 1 | 950 | 40 |
| Joint 2 | 900 | 20 |
| Joint 3 | 140 | 0 |
| Joint 4 | 160 | 3 |
According to my analysis, the reason for this difference is that I ignore the friction in the dynamic simulator. But in the real WAM, there are frictions in the joints. The frictions decrease the acceleration, thus prevent the joints from vibrating too much. This needs to be further tested on WAM.
5. Conclusion
In this project, a strategy of throwing trajectory and a dynamic simulation of the throwing motion of WAM are presented. A slow version of the trajectory is tested on the WAM. The experimental results and simulation results are compared. The result of the project shows that for a throwing problem, a velocity PID controller is a better choice. Using the trajectory in the project, the end effector’s speed can reach 6.5 m/s.
In the future, a friction effector can be considered to get the simulation more close to the real model. A higher speed trajectory and a throwing motion will be tested to see if the trajectory is suitable for throwing a ball or not.
6. References:
[1] R.M.Murray, Z.Li, S.S.Sastry, A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994
[2] Taku Senoo, Akio Namiki and Masatoshi Ishikawa. High-speed Throwing Motion Based on Kinetic Chain Approach. 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. (Nice, France, 2008.9.25) / Proceedings, pp.3206-3211.
Final Presentation
Anirudha Majumdar: I like the project because it plans a trajectory for the kicking motion. I would like to see a kicking motion that is closer to a human motion.
Avik De: It is cool to see the mathematical analyze of the motion planning. It would be better to see the motion to be tested on the real arm.
Ben Charrow: To see two robots cooperate to get through the door. It would be really interesting to see more robots cooperate.
Brian MacAllister: Controlling the robot with a dynamic path planning algorithm is a wonderful thing. The video is also very cool. My concern is that as it is a flying robot, did you include the dynamic model of the robot?
Caitlin Powers: I like the project because she uses two approach to control the arm and makes cool simulation. Just want to see the control algorithm on the WAM.
Ceridwen Magee: