def CreateIiwaControllerPlant():
"""creates plant that includes only the robot and gripper, used for controllers."""
robot_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf")
gripper_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")
sim_timestep = 1e-3
plant_robot = MultibodyPlant(sim_timestep)
parser = Parser(plant=plant_robot)
parser.AddModelFromFile(robot_sdf_path)
parser.AddModelFromFile(gripper_sdf_path)
plant_robot.WeldFrames(
A=plant_robot.world_frame(),
B=plant_robot.GetFrameByName("iiwa_link_0"))
plant_robot.WeldFrames(
A=plant_robot.GetFrameByName("iiwa_link_7"),
B=plant_robot.GetFrameByName("body"),
X_AB=RigidTransform(RollPitchYaw(np.pi/2, 0, np.pi/2), np.array([0, 0, 0.114]))
)
plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
plant_robot.Finalize()
link_frame_indices = []
for i in range(8):
link_frame_indices.append(
plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())
return plant_robot, link_frame_indices
def kinematic_tree_example():
plant = MultibodyPlant(time_step=0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf"
))
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
g = Source(plant.GetTopologyGraphvizString())
print(g.format)
g.view()
def grasp_poses_example():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
grasp = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"
), "grasp")
brick = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
meshcat = ConnectMeshcatVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
B_O = plant.GetBodyByName("base_link", brick)
X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)
B_Ggrasp = plant.GetBodyByName("body", grasp)
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
RotationMatrix.MakeZRotation(np.pi / 2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
X_WGgrasp = X_WO.multiply(X_OGgrasp)
plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
# Open the fingers, too
plant.GetJointByName("left_finger_sliding_joint",
grasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint",
grasp).set_translation(plant_context, 0.054)
meshcat.load()
diagram.Publish(context)
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
parser = Parser(plant)
if welded:
parser.package_map().Add(
"wsg_50_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/package.xml"))
)
gripper = parser.AddModelFromFile(
FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
else:
gripper = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf"))
X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
plant.GetFrameByName("body", gripper), X_7G)
return gripper
def num_positions_velocities_example():
plant = MultibodyPlant(time_step=0.0)
Parser(plant).AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
print(context)
print(f"plant.num_positions() = {plant.num_positions()}")
print(f"plant.num_velocities() = {plant.num_velocities()}")
def AddPlanarIiwa(plant):
urdf = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"planar_iiwa14_spheres_dense_elbow_collision.urdf")
parser = Parser(plant)
iiwa = parser.AddModelFromFile(urdf)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
q0 = [0.1, -1.2, 1.6]
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
def AddIiwa(plant, collision_model="no_collision"):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
f"iiwa7_{collision_model}.sdf")
parser = Parser(plant)
iiwa = parser.AddModelFromFile(sdf_path)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
q0 = [0.0, 0.1, 0, -1.2, 0, 1.6, 0]
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
def AddTwoLinkIiwa(plant, q0=[0.1, -1.2]):
urdf = FindResource("models/two_link_iiwa14.urdf")
parser = Parser(plant)
parser.package_map().Add(
"iiwa_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/package.xml")))
iiwa = parser.AddModelFromFile(urdf)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
import argparse
import math
import numpy as np
from pydrake.all import (Box, DiagramBuilder, FindResourceOrThrow,
FloatingBaseType, Isometry3, RigidBodyTree,
SignalLogger, Simulator, VisualElement)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
default=0.0)
parser.add_argument("-V",
"--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
parser.add_argument("-S",
"--slope",
type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
params.set_slope(args.slope)
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
default=10.0)
parser.add_argument("-Q", "--initial_angle",
type=float,
help="Initial angle of the stance leg (in radians).",
default=0.0)
parser.add_argument("-V", "--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
parser.add_argument("-S", "--slope", type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
params.set_slope(args.slope)
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
from pydrake.all import (ConstantVectorSource,
DiagramBuilder,
FindResourceOrThrow,
FloatingBaseType,
Isometry3,
RigidBodyTree,
SignalLogger,
Simulator,
VisualElement)
from pydrake.attic.multibody.shapes import Box
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
# plant.GetJointByName("iiwa_joint_4").set_angle(context, -1.2)
# # Set initial actuator values
# plant.get_actuation_input_port().FixValue(context, np.zeros(7))
# simulator = Simulator(plant, context)
# simulator.AdvanceTo(5.0)
builder = DiagramBuilder()
# Adds both MultibodyPlant and SceneGraph and wires them together
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Adds the MeshcatVisualizer and wires it to the Scene Graph
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, open_browser=True)
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
meshcat.load()
diagram.Publish(context)
plant_context = plant.GetMyMutableContextFromRoot(context)