How to get smooth movement from Arbotix controller Dynamixel Servos using ROS
I'm using ROS to communicate with a PhantomX Hexapod Robot that comes with an Arbotix-M and Dynamixel AX-18A Servos. To control the legs I want to use an Arbotix Action Client like so link text. To this, I then want to send a Joint Trajectory using trajectory_msgs.msg and control_msgs.msg to make the movement smooth.
My problem is that I cannot achieve a smooth trajectory from that. It seems like the movement to one trajectory point is devided into subsections at which the servo stops shortly like in this video link text
Setting up Action Client, making Trajectory and sending it to Client:
#!/usr/bin/env python
import roslib
import rospy
import actionlib
from std_msgs.msg import Float64
import trajectory_msgs.msg
import control_msgs.msg
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from geometry_msgs.msg import Twist
from copy import copy
class Joint:
def __init__(self, motor_name):
self.name = motor_name
self.jta = actionlib.SimpleActionClient('/controller/follow_joint_trajectory', FollowJointTrajectoryAction)
rospy.loginfo('Waiting for joint trajectory action')
self.jta.wait_for_server()
rospy.loginfo('Found joint trajectory action!')
self.goal = FollowJointTrajectoryGoal()
self.goal.trajectory.joint_names = ['j_c1_lf', 'j_c1_rf', 'j_thigh_lf', 'j_thigh_rf', 'j_tibia_lf', 'j_tibia_rf', 'j_c1_lr', 'j_c1_rr', 'j_thigh_lr', 'j_thigh_rr', 'j_tibia_lr', 'j_tibia_rr', 'j_c1_lm', 'j_c1_rm', 'j_thigh_lm', 'j_thigh_rm', 'j_tibia_lm', 'j_tibia_rm']
def add_point(self, angles, time, velo, accel):
point = JointTrajectoryPoint()
point.positions = copy(angles)
point.time_from_start = rospy.Duration(time)
point.velocities = copy(velo)
point.accelerations = copy(accel)
self.goal.trajectory.points.append(point)
def execute_trajectory(self):
self.goal.trajectory.header.stamp = rospy.Time.now()
self.jta.send_goal_and_wait(self.goal)
def main():
robot = Joint('controller')
robot.add_point([0.3, -0.9, -0.6, 0.6, 1.3, -1.3, -0.3, 0.3, -0.6, 0.6, 1.3, -1.3, 0.0, 0.0, -0.6, 0.6, 1.3, -1.3],1.0,[1.0]*18,[0.0]*18)
robot.add_point([0.3, -0.3, -0.6, 1.3, 1.3, -0.3, -0.3, 0.3, -0.6, 0.6, 1.3, -1.3,0.0, 0.0, -0.6, 0.6, 1.3, -1.3],2.0,[1.0]*18,[0.0]*18)
robot.add_point([0.28, 0.3, -0.6, 0.6, 1.3, -1.3, -0.3, 0.3, -0.6, 0.6, 1.3, -1.3,0.0, 0.0, -0.6, 0.6, 1.3, -1.3],3.0,[1.0]*18,[0.0]*18)
rospy.loginfo("trajectory has been filled: ")
rospy.loginfo(robot.goal)
for i in range(3):
robot.execute_trajectory()
robot.goal.trajectory.points = []
rospy.loginfo("trajectory has been emptied: ")
rospy.loginfo(robot.goal.trajectory.points)
if __name__ == '__main__':
rospy.init_node('joint_position_tester')
main()
Launch File:
<launch>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find odometrie)/configuration.yaml" command="load" />
</node>
</launch>
Controller Configuration YAML File:
port: /dev/ttyUSB0
rate: 15
joints: {
j_c1_lf: {id: 1, max_angle: 82, min_angle: -84}, j_c1_rf: {id: 2, max_angle: 82, min_angle: -83}, j_thigh_lf: {id: 3, max_angle: 101, min_angle: -103}, j_thigh_rf: {id: 4, max_angle: 103, min_angle: -101 ...
I'm learning to control the dynamixel motor as well. From the look of it, you only set up the start and stop and then execute it right away. The motor will work as a step motor, so it jump to the end position with max velocity and then stop. That will cause the jerk and then stop you see. To make a smooth movement, you should divide the trajectory into a number of positions that will generate a trapezoidal velocity profile or better yet, s-curve profile. You can see this answer for more information
https://robotics.stackexchange.com/qu...
Do you think it is a motion planning problem although I only send 3 trajectory points and the servos stop multiple times between those points? I'd be satisfied when the servos would go to each trajectory point with appropriate velocity.
No, I don't think there is any motion planning involve in this yet. (Not from the code you posted at least). By the way, from your code I see that you actually execute your trajectory 3 times with the for loop in main, not go to 3 points you added. Your trajectory will be like this: current position (you should always read the current position and add that to the trajectory to avoid jerk)-> first point -> second point > thirdpoint (first loop) -> first point -> second point -> third point (second loop) -> first point -> second point -> third point (third loop). I think that's why you see multiple stop, not just 3.
Yes, the leg moves to 9 points in total. But did you look at the video I provided? Between those points it stops multiple times. It seems like it separates the trajectory instead of adapting its velocity.
Ah, I see. I thought that wasn't your intention. You should look for the source code of the action server to see how they send the goal positions to the joints to see what's wrong. Since I haven't checked that yet, I can only guess from the behavior. It could be that the serve divide the trajectory its received into smaller segments with a number of points, and the moving time for each point is too much, it finish moving before the time ends, then stop, wait for next point. That causes multiple stops you see. And I don't think the serve do the velocity adaptation for you. To make it smooth, you must divide the trajectory your self, so that the velocity gradually increase between each step and never return to 0 between each points by control moving time for each segment. Google motion velocity ...(more)
Consider writing a server yourself, you can check this link for example. I wrote my own server based on this one https://github.com/ST-ROBOTICS/r12_ha...
Thank You madlink306 for your help. By now I figured out that the best way to control the servos in my setup is to create a python list with publishers for the '/command'-topics that control the servos and publish commands in a for-loop. That way the trajectory is not devided and the servos move to each point I publish smoothly. I also found a ros-service called /set_speed that allows me to control the velocity of each servo. However, because using services is quite time consuming I no longer try to separate the movement and accelerate.