Klamp't Tutorial: Send trajectories to a simulated robot

In this tutorial we will learn how to do a quick trajectory check on a simulated robot with RobotPose program and with python API

Difficulty: easy

Time: 5-20 minutes

You can generate trajectories from keyframes for a robot (check Tutorial 4 ). Once you have some trajectores saved as either .xml or .path files, you can reload them with RobotPose program. Here we have a simple trajectory generated with tx90 robot for you to test: tx90.xml tx90.path

First in Klampt folder, lauch the RobotPose program with the desired robot or world file. For this example, we use tx90scenario0.xml in Klampt/data folder.

./RobotPose data/tx90scenario0.xml

Then click "Load Resource" button to load the trajectory you want to check (either .xml file or .path file should work)

RobotPose screen 1

Now the trajectory is loaded, click the play button and the robot will be simulated with the given trajectory

RobotPose screen 2

Here using ./RobotPose you can check the motion. But to sent the motion to the real robot, you should use ./SimTest, please check Tutorial 2

The script we are using for this demo can be downloaded here: trajectory_demo. You will need a .path file, like this: tx90.path To run the code:

python trajectory_demo.py path/to/Klampt/data/tx90scenario0.xml path/to/tx90.path

Press 's' to start simulation. The robot will show up but won't move because the trajectory is loaded (on line 13-14) but not sent to the controller.

In Klampt, trajectory has two list of variables: milestones and times. Milestones is a list of joint values for the robot, where the timestamps are stored in the times. If you print out trajectory.milstones[1] and trajectory.times[1], you will get:

trajectory.milestones[1]=[0.0, -0.220207, 1.08004, -0.88, 1.61592e-05, 0.0, 0.0]
trajectory.times[1]=0.00994549

By default, the controller has three modes to control the robot:

  • Motion queue + PID mode: the controller has an internal trajectory queue that may be added to and modified. This queue supports piecewise linear interpolation, cubic interpolation, and time-optimal move-to commands.
  • PID mode: the user controls the motor's PID setpoints directly
  • Torque control: the user controlls the motor torques directly.

Let's first try to add the trajectory to the motion queue. Now copy and paste the following code right after line 14

for i in range(len(self.trajectory.times)):
	self.sim.controller(0).addMilestone(self.trajectory.milestones[i])

This is the easiest way to add milestones from a trajectory to the motion queue of the controller. However, if you run this you will see the robot moves really fast at the beginning to get to the first milestone, and then moves really slow afterwards. Using addMilestone function only sends the configurations (waypoints) to the robot, timing information is not used here. The list of milestones are interpolated dynamically from the current configuration/velocity. To handle disturbances, a PID loop is run automatically at the controller's specified rate. To get finer-grained control over the motion queue's timing, you may use the setLinear/setCubic/addLinear/addCubic functions. In these functions it is up to you to respect velocity, acceleration, and torque limits. Let's try setLinear now, replace the code you just pasted with the following code:

if vectorops.distance(self.sim.controller(0).getSensedConfig(),self.trajectory.milestones[0])>0.1:
    self.sim.controller(0).addLinear(self.trajectory.milestones[0],3)     
speed=1
for i in range(len(self.trajectory.times)):
    self.sim.controller(0).addLinear(self.trajectory.milestones[i+1],(self.trajectory.times[i+1]-self.trajectory.times[i])/speed)

The addLlinear(q,dt) function uses linear interpolation to get from the current configuration to the desired configuration(q) after time dt. The first two lines check the distance between current configuration of the robot and the starting configuration in the trajectory. The robot will move to the start configuration in 3 seconds. Then the trajectory is added to the motion queue. Now run and start the simulation again. You will see the robot moving as expected. You can also change the speed value to slow down or speed up the motion.

There's another way to send the trajectory to the robot. Instead of loading the trajectory when initing the program, you can load it inside the control loop. comment the code for adding milestones and copy the following code to function control_loop. Here traj.eval and traj.deriv functions calculate the q and dq based on the time. We are using a low level control setPIDCommand(qdes,dqdes) to control the robot. Whether in motion queue or PID mode, the constants of the PID loop are initially set in the robot file. You can programmatically overwrite the PID gains with setPIDGains(Not recommanded!).

sim = self.sim
traj = self.trajectory
starttime = 2
robotController = self.sim.controller(0)
if sim.getTime() > starttime:
    (q,dq) = (traj.eval(self.sim.getTime()-starttime),traj.deriv(self.sim.getTime()-starttime))
    robotController.setPIDCommand(q,dq)
else:
    robotController.setPIDCommand(traj.milestones[0],[0]*7)