Differences

This shows you the differences between two versions of the page.

--- ece4560:biped:04rrinvkin [2017/10/17 12:11] – created typos
+++ ece4560:biped:04rrinvkin [2024/08/20 21:38] (current) – external edit 127.0.0.1
@@ Line 8: / Line 8: @@
 <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, [ x_dot, y_dot, omega], 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 3x3 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.
+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:
@@ Line 14: / Line 14: @@
 <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 produce a joint trajectory to accomplish it.
+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 3xT matrix, [ x ; y ; theta ], where T is the number of waypoints comprising your trajectory and 'x', 'y', 'theta', here, are 1xT vectors. ''a_time'' is a 1xT vector holding timestamps associated with each waypoint in ''a_traj_ws''. The output, ''traj_alpha'', then, is a 3xT vector that should look something like [ hip_joint_traj ; knee_joint_traj ; ankle_joint_traj ] where 'hip_joint_traj', 'knee_joint_traj' and 'ankle_joint_traj' are 1xT vectors containing joint trajectories for each joint that is capable of affecting motion of the selected foot frame.
+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 3xT matrix, [ x ; y ; theta ], and 1xT 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).
+**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.
+**( a )** Plots of each relevant joint angle vs. time (ie. 3 plots)
-**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.
+**( 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''.
@@ Line 31: / Line 34: @@
 **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, [x_dot ; y_dot ; omega], where x_dot and y_dot are linear velocity components in the x- and y- directions (defined by the reference frame) and omega is an angular velocity (positive is determined by the right hand rule relative to the reference frame). Hence, the Jacobian will be a 3x3 matrix. Then, we have [x_dot ; y_dot ; omega] = J*alpha_dot, where alpha_dot is a column vector composed of your joint velocities (ie. one entry for each joint between the reference frame and end-effector frame).
+**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 2x3 portion of your Jacobian will map a 3-element joint velocity vector, alpha_dot, to the linear velocity ([ x_dot ;  y_dot ]) 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. 2x1 vector). Taking partial derivatives with respect to each joint angle then yields the upper 2x3 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)?
+**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'.
+**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:
@@ Line 43: / Line 46: @@
 **(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-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. [x_dot ; y_dot ; omega] = ([ 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 alpha_dot = inv(J)*[x_dot ; y_dot ; omega] (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 + alpha_dot*dt.
+**(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$.
 ---------