# (part 2)

MIT 6.4210/2: Robotic Manipulation

Fall 2022, Lecture 4

(or later at https://slides.com/russtedrake/fall22-lec04)

## Step 3: Forward kinematics of the iiwa + WSG

X^B = f_{kin}^B(q)

generalized positions

pose of body (or frame) $$B$$

## Check yourself: A single "free" body

plant = MultibodyPlant(time_step = 0.0)
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()

context = plant.CreateDefaultContext()

X^B = f_{kin}^B(q)

What is $$X^B$$ ?

X_B = plant.EvalBodyPoseInWorld(brick, context)

What is $$q$$ ?

q = plant.GetPositions(context)

## Step 4: Differential Kinematics

dX^B = \frac{\partial f_{kin}^B(q)}{\partial q} dq
= \underbrace{J^B(q)}dq
X^B = f_{kin}^B(q)

Kinematic "Jacobian"

## Spatial Velocity

\frac{d}{dt} {}^AX^B_C \equiv \, ^AV^B_C = \begin{bmatrix} {}^A\omega^B_C \\ {}^A\text{v}^B_C \end{bmatrix}

angular velocity

translational velocity

Q: How do we represent angular velocity, $$\omega^B$$??

(w_x, w_y, w_x)

A: Unlike 3D rotations, here 3 numbers are sufficient and efficient; so we use the same representation everywhere.

note the typesetting

## Check yourself: A single "free" body

plant = MultibodyPlant(time_step = 0.0)
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()

context = plant.CreateDefaultContext()

\dot{q} = N(q) v

What is $$v$$ ?

v = plant.GetVelocities(context)

What is $$q$$ ?

q = plant.GetPositions(context)
v = plant.MapQDotToVelocity(context, qdot)
qdot = plant.MapVelocityToQDot(context, v)

For any MultibodyPlant (not just a single body):

## Kinematics

• Forward kinematics:    joint positions $$\Rightarrow$$ pose
• Inverse kinematics*:      pose $$\Rightarrow$$ joint positions

• Differential kinematics:
joint positions, velocities $$\Rightarrow$$ spatial velocity

• Differential inverse kinematics:
spatial velocity, joint positions $$\Rightarrow$$ joint velocities

q \Rightarrow X^B
X^B \Rightarrow q
q, v \Rightarrow V^B
V^B, q \Rightarrow v

## Our first Jacobian pseudo-inverse controller

plant.SetPositions(plant_context, iiwa, q)
J_G = plant.CalcJacobianSpatialVelocity(
plant_context, JacobianWrtVariable.kQDot,
G, [0,0,0], self._W, self._W)
J_G = J_G[:,0:7] # Ignore gripper terms

V_G_desired = np.array([0,    # rotation about x
0,    # x
-.05, # y
-.1]) # z
v_iiwa = np.linalg.pinv(J_G).dot(V_G_desired)

prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
result = Solve(prog)
{"found_math": true, "script_math_tex": 0, "script_math_asciimath": 0, "math_annotations": 0, "math_alttext": 0, "mathml": 0, "mathjax_tag": 0, "mathjax_inline_tex": 0, "mathjax_display_tex": 1, "mathjax_asciimath": 1, "img_math": 0, "codecogs_latex": 0, "wp_latex": 0, "mimetex.cgi": 0, "/images/math/codecogs": 0, "mathtex.cgi": 0, "katex": 0, "math-container": 0, "wp-katex-eq": 0, "align": 0, "equation": 0, "x-ck12": 0, "texerror": 0, "url": "https://slides.com/russtedrake/fall22-lec04", "fetch_time": 1726818824000000000, "content_mime_type": "text/html", "warc_filename": "crawl-data/CC-MAIN-2024-38/segments/1725700652138.47/warc/CC-MAIN-20240920054402-20240920084402-00152.warc.gz", "warc_record_offset": 481414267, "warc_record_length": 14588, "token_count": 762, "char_count": 2462, "score": 3.609375, "int_score": 4, "crawl": "CC-MAIN-2024-38", "snapshot_type": "latest", "language": "en", "language_score": 0.5196453928947449, "file_path": "/sailhome/yjruan/project/latent-lm/data/processed/finemath-4plus/finemath-4plus/train-00002-of-00064.parquet"}