How to transfer base velocities to a differential drive robot?

[aman-ga]

I am running Stretch Hello Robot example.
I noticed that base movement is controlled by first 3 components velocities given by IK solve.
first 2 component give x and y velocities to robot which makes the robot sliding in xy plane.
how do i transfer velocities to a differential drive format ( v and w )

[stephane-caron]

You did not specify what $v$ and $w$ are but I'm guessing $v$ is the linear (longitudinal) velocity and $w$ is the angular velocity of the differential-drive model. In that case you can read them straightaway from the velocity vector that is computed at these lines of the example:

        # Compute velocity and integrate it into next configuration
        velocity = solve_ik(configuration, tasks, dt, solver=solver)
        configuration.integrate_inplace(velocity, dt)

To understand the structure of this vector, let's check:

In [2]: robot.nv
Out[7]: 17

The Stretch robot itself has 14 actuated joints (checked e.g. from the URDF file directly, or from the robot description index here). On top of this we added a planar joint at this step:

    robot = load_robot_description(
        "stretch_description", root_joint=pin.JointModelPlanar()
    )

The planar joint adds three velocity coordinates to velocity vectors. The structure of the velocity vector is then:

• velocity[0:3]: floating-base (a.k.a. root joint) velocity:
  • velocity[0]: linear velocity $v_x$ (x-axis of the world frame)
  • velocity[1]: linear velocity $v_y$ (y-axis of the world frame)
  • velocity[2]: angular velocity $\omega_z$ (z-axis of the world frame)
• velocity[3:]: actuated joint velocities.

If I understand correctly your $v$ and $w$, you could then compute them as, for instance:

v = np.linalg.norm(velocity[0:2])
w = velocity[2]

Hoping this helps!