This shows examples of:
MultibodyPlant containing an IIWA armMathematicalProgram that can handle both float and AutoDiffXd
inputsTo 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.math import RigidTransform
from pydrake.multibody.parsing import Parser
from pydrake.systems.analysis import Simulator
from pydrake.all import MultibodyPlant
from pydrake.solvers import MathematicalProgram, Solve
plant_f = MultibodyPlant(0.0)
iiwa_url = (
"package://drake_models/iiwa_description/sdf/iiwa14_no_collision.sdf"
)
(iiwa,) = Parser(plant_f).AddModels(url=iiwa_url)
# 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 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)
<pydrake.solvers.Binding[Constraint] at 0x79cf86ce2f70>
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}")
Success? True SolutionResult.kSolutionFound [-0.19321514 0.68565392 -0.42294188 1.29787137 -0.02183773 -0.32328291 0. ] Initial distance: 0.457 Solution distance: 0.200
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)
<pydrake.solvers.Binding[Cost] at 0x79cf86e6a670>
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}")
Success? True SolutionResult.kSolutionFound [ 2.23631424 1.26603879 0.56411348 -3.6261624 0.01314088 1.36259448 0. ] Initial distance: 0.457 Solution distance: 0.000