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.

To be added:

  • 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")
iiwa = Parser(plant_f).AddModelFromFile(iiwa_file)

# 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.
plant_ad = plant_f.ToAutoDiffXd()
context_ad = plant_ad.CreateDefaultContext()

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]

def link_7_distance_to_target(q):
    """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.
        plant = plant_ad
        context = context_ad
    # 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"
link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)]

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.
prog.AddConstraint(
    link_7_distance_to_target_vector,
    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)

print(f"Initial distance: {link_7_distance_to_target(q0):.3f}")
print(f"Solution distance: {link_7_distance_to_target(q_sol):.3f}")

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.
prog.AddCost(link_7_distance_to_target, 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)

print(f"Initial distance: {link_7_distance_to_target(q0):.3f}")
print(f"Solution distance: {link_7_distance_to_target(q_sol):.3f}")