Hi!
I am trying to control the mobile base using the stretch_body API, but I am pretty confused as to how to chain trajectories together.
Few questions:
- How to properly use the trajectory following API? (Especially for the base). Am I using it correctly? (The 3rd action is not getting executed) Snippet is attached below.
- When translating the base forward/backward, the motion has a lot of jerks. Is there any way to change the gains or any parameters that can be tuned to get a better trajectory?
b = stretch_body.base.Base()
b.left_wheel.disable_sync_mode()
b.right_wheel.disable_sync_mode()
b.startup()
def addrot(b, prevx, prevy, prevt, theta, duration):
b.trajectory.add(
time=prevt + duration,
x=prevx,
y=prevy,
theta=np.deg2rad(theta),
translational_vel=0,
rotational_vel=0,
translational_accel = 0,
rotational_accel = 0,
)
return prevx, prevy, theta, prevt + duration
def addtranslate(b, prevx, prevy,prevt, deltax, deltay, prevtheta, duration):
x = prevx + deltax
y = prevy + deltay
t = prevt + duration
b.trajectory.add(
time=prevt + duration,
x=prevx + deltax,
y = deltay + prevy,
theta=prevtheta,
translational_vel=0,
rotational_vel=0,
translational_accel = 0,
rotational_accel = 0,
)
return x, y, prevtheta, t
x = 0.0
y = 0.0
theta = 0.0
t = 0.0
b.first_step = True
b.trajectory.add(time=0, x=0, y=0, theta=0)
x, y, theta, t = addtranslate(b, x, y, t, 0.5, 0.0, theta, 5)
x, y, theta, t = addrot(b, x, y, t, -90, 5)
x, y, theta, t = addtranslate(b, x, y, t, 0.5, 0.0, theta, 2) # not executing
b.follow_trajectory()
starttime = time.time()
while time.time() - starttime < t + 3:
time.sleep(1)
b.pretty_print()
b.stop()
print('done')
exit(0)
