Mobile base trajectories

[hello-binit]

This PR makes the trajectory interface available for the mobile base. In order to support trajectories composed of translations and rotations, a new trajectory class DiffDriveTrajectory is defined. Waypoints are given as SE2 poses (x, y, theta) of the base. There must only be translation or rotation between two of these poses. These waypoints are converted into motor space linear trajectories, and sent simultaneously to left and right wheels to perform coordinated motion.

Unit tests of single and multi DOF trajectories are part of this PR and tested on a robot in Ubuntu 18.04/Python 2. An example program is reproduced here:

import stretch_body.base
import numpy as np

b = stretch_body.base.Base()
b.left_wheel.disable_sync_mode()
b.right_wheel.disable_sync_mode()
b.startup()

b.trajectory.add(time=0, x=0, y=0, theta=0, translation_vel=0, rotational_vel=0, translation_accel=0, rotational_vel=0)
b.trajectory.add(time=6, x=0.4, y=0, theta=0, translation_vel=0, rotational_vel=0, translation_accel=0, rotational_vel=0)
b.trajectory.add(time=12, x=0.4, y=0, theta=np.pi, translation_vel=0, rotational_vel=0, translation_accel=0, rotational_vel=0)
b.trajectory.add(time=18, x=0, y=0, theta=np.pi, translation_vel=0, rotational_vel=0, translation_accel=0, rotational_vel=0)
b.trajectory.add(time=24, x=0, y=0, theta=0, translation_vel=0, rotational_vel=0, translation_accel=0, rotational_vel=0)
b.follow_trajectory()
time.sleep(25)

b.stop()

The trajectory above sends the robot forward 0.4m, rotates around 180 degrees, forward again 0.4m, rotates again 180 degrees, ending up exactly where the robot started off.

[praveenVnktsh]

@hello-binit A quick query - in this, what does the following snippet do?
b.left_wheel.disable_sync_mode() b.right_wheel.disable_sync_mode()

I am trying to create a piecewise trajectory that can control the base. However, I am unable to control it if I don't move the wrist as well. Does this have something to do with the sync mode?

[hello-binit]

Hi @praveenVnktsh, good question! Motion of Stretch's four main joints are synchronized. These four joints are left wheel, right wheel, lift, and arm. When you issue a command to these joints, they don't start execution immediately. They wait for a "synchronizing pulse" at the embedded level to start motion in unison with each other. For example, this is why the robot moves perfectly forward when you command the base to move forward 1 meter; if there was no pulse, the left wheel might start a few milliseconds before the right wheel starts moving, resulting in the robot skewing right.

The sync pulse comes from a PCB called the "pimu", and you can interact with the pimu from Stretch Body to manually issue pulses using import stretch_body.pimu; p = stretch_body.pimu.Pimu(); p.trigger_motor_sync(); p.push_command(). Since I wanted to keep the snippet above minimal, I didn't want to add code related to the pimu. Therefore, I disabled the "wait for sync pulse" behavior of the left and right wheels by calling b.left_wheel.disable_sync_mode() b.right_wheel.disable_sync_mode().

The wrist isn't a Hello Robot embedded device, it's actually a Dynamixel device, so it doesn't know or care about sync pulses. So there shouldn't be any reason why the base would refuse to execute a piecewise trajectory until the wrist moves. If you're willing to share the snippet you're developing, you can create a new topic on our Forum, and I'd be happy to help you with it.