====== Bipedal Robot Locomotion: Leg Resolved Rate Inverse Kinematics  ======
----------------------------------------------------------------

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:

<code>[ J ] = jacobian( obj, a_alpha, a_ref_frame, a_ee_frame )</code>

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: 

<code>[ traj_alpha ] = rr_inv_kin( obj, a_traj_ws, a_time, a_ref_frame, a_ee_frame )</code>

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$.  

---------
;#;
[[ ECE4560:Biped:Adventures | Back]] -- [[: | Main ]]
;#;