This week we'll use resolved rate inverse kinematics to generate joint trajectories that accomplish desired planar (workspace) trajectories for a frame on the foot of your biped. This is a useful tool and the associated tasks will take you a step closer to generating viable planar bipedal walking gaits.

1. Add a new method to your Biped class:

[ J ] = jacobian( obj, a_alpha, a_ref_frame, a_ee_frame )

The output, J, will be a 3-by-N matrix that maps joint velocities, alpha_dot (vector of length, N), to velocity twist coordinates, [ $\dot{x}$, $\dot{y}$, $\dot{\theta}$], representing velocity of an end-effector frame (ie. a_ee_frame), with respect to the reference frame (ie. a_ref_frame). For this week's purposes, we will fix a_ref_frame to be TORSO and a_ee_frame will be your choice of either LEFT_FOOT or RIGHT_FOOT. In other words, jacobian( … ) should return the $3 \times 3$ matrix that maps joint velocities, alpha_dot (vector of length 3), to the corresponding velocity twist describing motion of the (selected) foot frame, relative to the TORSO frame. a_alpha is a 3-element vector containing joint positions for all joints between the end-effector and reference frames.

2. Add a new method to your Biped class:

[ traj_alpha ] = rr_inv_kin( obj, a_traj_ws, a_time, a_ref_frame, a_ee_frame )

This method will take a desired (workspace) trajectory of your (selected) foot frame, relative to the TORSO frame, and produces a joint trajectory to accomplish it. The same assumptions, from #1, hold for inputs a_ref_frame and a_ee_frame. a_traj_ws represents the foot frame trajectory (with respect to the a_ref_frame = TORSO frame). This is a $3 \times T$ matrix, [ $x$ ; $y$ ; $\theta$ ], where $T$ is the number of waypoints comprising your trajectory and $x$, $y$, $\theta$, here, are $1 \times T$ vectors. a_time is a $1 \times T$ vector holding timestamps associated with each waypoint in a_traj_ws. The output, traj_alpha, then, is a $3 \times T$ vector that should look something like [ $\alpha_{hip}$ ; $\alpha_{knee}$ ; $\alpha_{ankle}$ ] where $\alpha_{hip}$, $\alpha_{knee}$ and $\alpha_{ankle}$ are $1 \times T$ vectors containing joint trajectories for each joint that is capable of affecting motion of the selected foot frame.

3. Pick a leg (either RIGHT_FOOT or LEFT_FOOT). Generate a trajectory for that foot frame relative to the TORSO frame (ie. assuming the TORSO frame is locked to and aligned with the spatial/origin frame); this is a $3 \times T$ matrix, [ $x$ ; $y$ ; $\theta$ ], and $1 \times T$ timestamp matrix (this can start from $t = 0$ sec.). Pass these and any other needed inputs to rr_inv_kin( … ) to produce a joint trajectory that accomplishes the prescribed workspace trajectory for the foot frame (again, relative to the TORSO frame).

Generate the following:

( a ) Plots of each relevant joint angle vs. time (ie. 3 plots)

( b ) Plot of $x$-$y$ foot position over time. For 3 different time instants, overlay a visualization of the foot frame on this plot.

( c ) Overlay the $x$-$y$ foot position over time (again, relative to the TORSO frame or assuming TORSO frame is locked to and aligned with the spatial/origin axes) produced by the resolved rate inverse kinematics joint trajectory, traj_alpha. You will have to perform forward kinematics (eg. using fwd_kinematics( … )) to obtain the the actual accomplished foot frame trajectory.

4. Use animateTrajectory( … ) to produce an animation of your resolved rate inverse kinematics joint trajectory, traj_alpha.

5. Let's say that instead of using TORSO as our reference frame, we use RIGHT_FOOT. We additionally set our end-effector frame to be LEFT_FOOT and generate a (workspace) trajectory that we want our LEFT_FOOT frame to follow, relative to the RIGHT_FOOT frame (ie. we now assume RIGHT_FOOT frame is locked to and aligned with the spatial/origin frame). How many joints of your biped are now capable of impacting motion of the LEFT_FOOT frame relative to the RIGHT_FOOT frame? What would be the new size of your Jacobian matrix? What kind of problems might this pose to performing resolved rate inverse kinematics? How might using a pseudo-inverse help in this situation?

Notes/Hints:

a.) The Jacobian, J, will map a 3-element vector of joint velocities (alpha_dot) to a 3-element vector representing velocity twist of the end-effector (relative to the selected reference frame). This 3-element vector will have components, [$\dot{x}$ ; $\dot{y}$ ; $\dot{\theta}$], where $\dot{x}$ and $\dot{y}$ are linear velocity components in the $x$- and $y$- directions (defined by the reference frame) and $\dot{\theta}$ is an angular velocity (positive is determined by the right hand rule relative to the reference frame). Hence, the Jacobian will be a $3 \times 3$ matrix. Then, we have [$\dot{x}$ ; $\dot{y}$ ; $\dot{\theta}$] = $J \dot{\alpha}$, where $\dot{\alpha}$ is a column vector composed of your joint velocities (ie. one entry for each joint between the reference frame and end-effector frame).

b.) Since you have not covered resolved rate inverse kinematics in lecture yet, we'll take a bit of a 'hacky' approach to constructing the Jacobian. The upper $2 \times 3$ portion of your Jacobian will map a 3-element joint velocity vector, $\dot{\alpha}$, to the linear velocity ([ $\dot{x}$ ; $\dot{y}$ ]) of your end-effector frame. You can obtain this in closed-form (as a function of your current joint configuration, $\alpha$) by first composing the $SE(2)$ matrix representing the pose of your end-effector frame (relative to the reference frame); this is just forward kinematics, which you've done in previous weeks. The translational coordinates of this $SE(2)$ pose, then, are the closed-form expression for the $x$-$y$ coordinates of your end-effector frame relative to the reference frame (ie. $2 \times 1$ vector). Taking partial derivatives with respect to each joint angle then yields the upper $2 \times 3$ block of your Jacobian matrix. The last row of your Jacobian matrix maps alpha_dot to the angular velocity of the end-effector frame; what should this row look like (ie. how do you obtain, in closed-form, the orientation of the end-effector frame, relative to the reference frame, given the joint angles in between the two frames; now take partial derivatives with respect to each joint angle)?

c.) When generating your prescribed workspace trajectories using this method, try to avoid singular configurations. A good rule of thumb may be to avoid configurations where all links are outstretched in a single line as well as configurations where links completely fold-over/overlap onto one another. You may want to begin your leg (at the start of the trajectory) in a configuration where joint angles are not all simultaneously set to $0$.

d.) To perform resolve rate inverse kinematics, try this sequence of steps:

(d-1) Set your reference frame to TORSO

(d-2) Pick an end-effector (eg. LEFT_FOOT or RIGHT_FOOT)

(d-3) With respect to your reference frame, define a 2-D trajectory/path for your foot frame to follow. You may define this as a sequenced array of $x$-$y$ coordinates representing foot positions at discrete instants of time. Append a corresponding third coordinate to your trajectory representing the relative orientation your foot frame should follow over time (eg. varying within [$-\pi$, $\pi$]). I'd suggest defining a constant time period between consecutive positions, eg. $dt = 0.1$ seconds.

(d-4) Loop through your trajectory array. On each iteration, compute the twist required to move toward the next waypoint in the array, eg. [$\dot{x}$ ; $\dot{y}$ ; $\dot{\theta}$] = ([ $x_{i+1} - x_i$ ; $y_{i+1} - y_i$ ; $\theta_{i+1} - \theta_i$]) / $dt$. Then compute the Jacobian, $J$ (remember $J$ will depend on your current joint configuration). Next, try computing $\dot{\alpha}$ = $J^{-1}$ [$\dot{x}$ ; $\dot{y}$ ; $\dot{\theta}$] (you may opt to use pinv() if you run into singularity issues). Prior to the next iteration, update your joint configuration, $\alpha_{i+1} = \alpha_i + \dot{\alpha} \cdot dt$.

Back – Main