This shows examples of:
MultibodyPlant
containing an IIWA armfor MathematicalProgram
that can handle both float
and AutoDiffXd
inputs
*To be added*:
pydrake.multibody.inverse_kinematics
.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.
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.
First, we will import the necessary modules and load a MultibodyPlant
containing an IIWA.
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
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()
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
.
# 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)]
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.
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)
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}")
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.
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)
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}")