# Mathematical Program MultibodyPlant Tutorial¶

For instructions on how to run these tutorial notebooks, please see the README.

This shows examples of:

• Creating a MultibodyPlant containing an IIWA arm
• Solve a simple inverse kinematics problem by writing a custom evaluator for MathematicalProgram that can handle both float and AutoDiffXd inputs
• Using the custom evaluator in a constraint
• Using the custom evaluator in a cost.

• Using pydrake.multibody.inverse_kinematics.
• Visualizing with Drake Visualizer.

### Important Note¶

Please review the API for pydrake.multibody.inverse_kinematics before you delve too far into writing custom evaluators for use with MultibodyPlant. You may find the functionality you want there.

## Inverse Kinematics Problem¶

In this tutorial, we will be solving a simple inverse kinematics problem to put Link 7's origin at a given distance from a target position. We will use MathematicalProgram to solve this problem in two different ways: first using the evaluator as a constraint (with a minimum and maximum distance), and second using the evaluator as a cost (to get as close as possible).

For more information about MathematicalProgram, please see the MathematicalProgram Tutorial.

## Setup¶

First, we will import the necessary modules and load a MultibodyPlant containing an IIWA.

In [ ]:
import numpy as np

from pydrake.common import FindResourceOrThrow
from pydrake.math import RigidTransform
from pydrake.multibody.parsing import Parser
from pydrake.systems.analysis import Simulator
from pydrake.all import MultibodyPlant

from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve

In [ ]:
plant_f = MultibodyPlant(0.0)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")

# Define some short aliases for frames.
W = plant_f.world_frame()
L0 = plant_f.GetFrameByName("iiwa_link_0", iiwa)
L7 = plant_f.GetFrameByName("iiwa_link_7", iiwa)

plant_f.WeldFrames(W, L0)
plant_f.Finalize()


## Writing our Custom Evaluator¶

Our evaluator is implemented using the custom evaluator link_7_distance_to_target, since the its functionality is not already handled by existing classes in the inverse_kinematics submodule.

Note that in order to write a custom evaluator in Python, we must explicitly check for float and AutoDiffXd inputs, as you will see in the implementation of link_7_distance_to_target.

In [ ]:
# Allocate float context to be used by evaluators.
context_f = plant_f.CreateDefaultContext()
# Create AutoDiffXd plant and corresponding context.

def resolve_frame(plant, F):
"""Gets a frame from a plant whose scalar type may be different."""
return plant.GetFrameByName(F.name(), F.model_instance())

# Define target position.
p_WT = [0.1, 0.1, 0.6]

"""Evaluates squared distance between L7 origin and target T."""
# Choose plant and context based on dtype.
if q.dtype == float:
plant = plant_f
context = context_f
else:
# Assume AutoDiff.
# Do forward kinematics.
plant.SetPositions(context, iiwa, q)
X_WL7 = plant.CalcRelativeTransform(
context, resolve_frame(plant, W), resolve_frame(plant, L7))
p_TL7 = X_WL7.translation() - p_WT
return p_TL7.dot(p_TL7)

# WARNING: If you return a scalar for a constraint, or a vector for
# a cost, you may get the following cryptic error:
# "Unable to cast Python instance to C++ type"


## Formulating the Optimization Problems¶

### Formluation 1: Using the Custom Evaluator in a Constraint¶

We will formulate and solve the problem with a basic cost and our custom evaluator in a constraint.

Note that we use the vectorized version of the evaluator.

In [ ]:
prog = MathematicalProgram()

q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration.
q0 = np.zeros(plant_f.num_positions())

# Add basic cost. (This will be parsed into a QuadraticCost.)
prog.AddCost((q - q0).dot(q - q0))

# Add constraint based on custom evaluator.
lb=[0.1], ub=[0.2], vars=q)

In [ ]:
result = Solve(prog, initial_guess=q0)

print(f"Success? {result.is_success()}")
print(result.get_solution_result())
q_sol = result.GetSolution(q)
print(q_sol)



### Formulation 2: Using Custom Evaluator in a Cost¶

We will formulate and solve the problem, but this time we will use our custom evaluator in a cost.

Note that we use the scalar version of the evaluator.

In [ ]:
prog = MathematicalProgram()

q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration.
q0 = np.zeros(plant_f.num_positions())

# Add custom cost.

result = Solve(prog, initial_guess=q0)