def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
Tview=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def build_pancake_flipper_plant(builder):
'''Creates a pancake_flipper MultibodyPlant. Returns the plant and
corresponding scene graph.
Inputs:
builder -- a DiagramBuilder
Outputs:
the MultibodyPlant object and corresponding SceneGraph representing
the pancake flipper
'''
# Instantiate the pancake flipper plant and the scene graph.
# The scene graph is a container for the geometries of all
# the physical systems in our diagram
pancake_flipper, scene_graph = AddMultibodyPlantSceneGraph(
builder,
time_step=0.0 # discrete update period, or zero for continuous systems
)
# parse the urdf and populate the plant
urdf_path = './models/pancake_flipper_massless_links_nofriction.urdf'
Parser(pancake_flipper).AddModelFromFile(urdf_path)
# Finalize the plant so we can use it
pancake_flipper.Finalize()
# Return the plant and scene graph, as promised
return pancake_flipper, scene_graph
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port,
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(
plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def __init__(self,
builder,
dt=5e-4,
N=150,
params=None,
trj_decay=0.7,
x_w_cov=1e-5,
door_angle_ref=1.0,
visualize=False):
self.plant_derivs = MultibodyPlant(time_step=dt)
parser = Parser(self.plant_derivs)
self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
parser,
params=params)
self.plant_derivs.Finalize()
self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
self.plant_derivs.get_actuation_input_port().FixValue(
self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
self.plant_derivs_context, null_force)
self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=dt)
parser = Parser(self.plant, scene_graph)
self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
parser,
params=params)
self.plant.Finalize(
) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.
self.meshcat = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=zmq_url)
self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff
self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls
nq = self.plant.num_positions()
nv = self.plant.num_velocities()
self.n_x = nq + nv
self.n_u = self.plant.num_actuators()
self.n_y = self.plant.get_state_output_port(self.iiwa).size()
self.N = N
self.dt = dt
self.decay = trj_decay
self.V = 1e-2 * np.ones(self.n_y)
self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
self.x_w_cov = x_w_cov
self.door_angle_ref = door_angle_ref
self.q0 = np.array(
[-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
#self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
self.x0 = np.concatenate((self.q0, np.zeros(nv)))
self.door_index = None
self.phi = {}
def RunSimulation(self, real_time_rate=1.0):
'''
Here we test using the NNSystem in a Simulator to drive
an acrobot.
'''
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def compile_scene_tree_clearance_geometry_to_mbp_and_sg(scene_tree, timestep=0.001, alpha=0.25):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=timestep))
parser = Parser(mbp)
world_body = mbp.world_body()
node_to_body_id_map = {}
free_body_poses = []
# For generating colors.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
for node in scene_tree.nodes:
if isinstance(node, SpatialNodeMixin) and isinstance(node, PhysicsGeometryNodeMixin):
# Don't have to do anything if this does not introduce geometry.
has_clearance_geometry = len(node.clearance_geometry) > 0
if not has_clearance_geometry:
continue
# Add a body for this node and register the clearance geometry.
# TODO(gizatt) This tree body index is built in to disambiguate names.
# But I forsee a name-to-stuff resolution crisis when inference time comes...
# this might get resolved by the solution to that.
body = mbp.AddRigidBody(name=node.name + "_%04d" % mbp.num_bodies(),
M_BBo_B=node.spatial_inertia)
node_to_body_id_map[node] = body.index()
tf = torch_tf_to_drake_tf(node.tf)
mbp.SetDefaultFreeBodyPose(body, tf)
# Pick out a color for this class.
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = list(cmap(cmap_counter))
color[3] = alpha
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi*2., 1.)
# Handle adding primitive geometry by adding it all to one
# mbp.
if len(node.clearance_geometry) > 0:
for k, (tf, geometry) in enumerate(node.clearance_geometry):
mbp.RegisterCollisionGeometry(
body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name + "_col_%03d" % k,
coulomb_friction=default_friction)
mbp.RegisterVisualGeometry(
body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name + "_vis_%03d" % k,
diffuse_color=color)
return builder, mbp, scene_graph
def setup_dot_diagram(builder, args):
''' Using an existing DiagramBuilder, adds a sim of the Dot
robot. Args comes from argparse.
The returned controller will need its first port connected to
a setpoint source.'''
print(
"TODO: load in servo calibration dict to a servo calibration object that gets shared"
)
with open(args.yaml_path, "r") as f:
config_dict = yaml.load(f, Loader=Loader)
sdf_path = os.path.join(os.path.dirname(args.yaml_path),
config_dict["sdf_path"])
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005)
parser = Parser(plant)
model = parser.AddModelFromFile(sdf_path)
# Set initial pose floating above the ground.
plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"),
RigidTransform(p=[0., 0., 0.25]))
if args.welded:
plant.WeldFrames(plant.world_frame(),
plant.GetBodyByName("body").body_frame())
else:
add_ground(plant)
plant.Finalize()
controller = builder.AddSystem(ServoController(plant, config_dict))
# Fixed control-rate controller with a low pass filter on its torque output.
zoh = builder.AddSystem(
ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos))
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(1))
builder.Connect(controller.get_output_port(0), zoh.get_input_port(0))
builder.Connect(zoh.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
plant.get_actuation_input_port())
return plant, scene_graph, controller
def __init__(self, file=None, terrain=FlatTerrain(), dlevel=1):
"""
Initialize TimeSteppingMultibodyPlant with a model from a file and an arbitrary terrain geometry. Initialization also welds the first frame in the MultibodyPlant to the world frame
"""
self.builder = DiagramBuilder()
self.multibody, self.scene_graph = AddMultibodyPlantSceneGraph(
self.builder, 0.001)
# Store the terrain
self.terrain = terrain
self._dlevel = dlevel
# Build the MultibodyPlant from the file, if one exists
self.model_index = []
if file is not None:
# Parse the file
self.model_index = Parser(self.multibody).AddModelFromFile(
FindResource(file))
# Initialize the collision data
self.collision_frames = []
self.collision_poses = []
self.collision_radius = []
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 main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
body = add_body(plant, "welded_body")
add_geometry(plant, body)
plant.WeldFrames(plant.world_frame(), body.body_frame())
body = add_body(plant, "floating_body")
add_geometry(plant, body)
plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.]))
plant.Finalize()
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_context()
# plant.SetFreeBodyPose(context, body, X_WB)
# plant.SetFreeBodySpatialVelocity(body, V_WB, context)
# Should look at, show 40sec to 50sec.
# TODO(eric.cousineau): Without reset stats, it freezes? :(
# simulator.AdvanceTo(40.)
simulator.set_target_realtime_rate(100.)
# simulator.ResetStatistics()
dt = 0.1
while context.get_time() < 240.:
simulator.AdvanceTo(context.get_time() + dt)
def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
path = subprocess.check_output([
'venv/share/drake/common/resource_tool',
'-print_resource_path',
'drake/examples/multibody/cart_pole/cart_pole.sdf',
]).strip()
Parser(plant).AddModelFromFile(path)
plant.Finalize()
# Add to visualizer.
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Add controller.
controller = builder.AddSystem(BalancingLQR(plant))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
# Set up a simulator to run this diagram.
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
# Simulate.
duration = 8.0
for i in range(5):
initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4)
print(f"Iteration: {i}. Initial: {initial_state}")
context.SetContinuousState(initial_state)
context.SetTime(0.0)
simulator.Initialize()
simulator.AdvanceTo(duration)
Our robot has two bodies:
1. The base. This moves on a 1D rail and oscillates according to the harmonic law $h \sin (\omega t)$.
2. The pendulum. It is connected to the base through a pin. This is the body you will need to control.
**Attention!** Since the robot has two bodies, it also has two configuration variables.
When writing the controller, we will take care of the first (position of the base) and ensure that it oscillates as required.
Then the problem will be reduced to the control of the pendulum only.
"""
# think of the builder as the construction site of our block diagram
builder = DiagramBuilder()
# instantiate the vibrating pendulum and the scene graph
# the scene graph is a container for the geometries of all the physical systems in our diagram
vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
builder,
time_step=0. # discrete update period , set to zero since system is continuous
)
# parse the urdf and populate the vibrating pendulum
urdf_path = FindResource('models/vibrating_pendulum.urdf')
Parser(vibrating_pendulum).AddModelFromFile(urdf_path)
vibrating_pendulum.Finalize()
"""## Write the Controller
In this section we define two controllers:
1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you.
2. The outer controller to make the pendulum spin at constant velocity. You will write part of this.
The final diagram will have the following structure:
from pydrake.all import (AddMultibodyPlantSceneGraph,
DiagramBuilder,
Parser,
Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
'''
mbp.RegisterVisualGeometry(body, pose, shape, name + "_vis", color)
mbp.RegisterCollisionGeometry(body, pose, shape, name + "_col",
friction)
if __name__ == "__main__":
# Random seed setup for reproducibility.
np.random.seed(int(codecs.encode(os.urandom(4), 'hex'), 32) & (2**32 - 1))
random.seed(os.urandom(4))
for scene_k in range(25):
# Set up a new Drake scene from scratch.
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.005))
# Add "tabletop" (ground) as a fixed Box welded to the world.
world_body = mbp.world_body()
ground_shape = Box(1., 1., 1.)
ground_body = mbp.AddRigidBody("ground", SpatialInertia(
mass=10.0, p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform())
RegisterVisualAndCollisionGeometry(
mbp, ground_body,
RigidTransform(p=[0, 0, -0.5]),
ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]),
CoulombFriction(0.9, 0.8))
class TimeSteppingMultibodyPlant():
"""
"""
def __init__(self, file=None, terrain=FlatTerrain(), dlevel=1):
"""
Initialize TimeSteppingMultibodyPlant with a model from a file and an arbitrary terrain geometry. Initialization also welds the first frame in the MultibodyPlant to the world frame
"""
self.builder = DiagramBuilder()
self.multibody, self.scene_graph = AddMultibodyPlantSceneGraph(
self.builder, 0.001)
# Store the terrain
self.terrain = terrain
self._dlevel = dlevel
# Build the MultibodyPlant from the file, if one exists
self.model_index = []
if file is not None:
# Parse the file
self.model_index = Parser(self.multibody).AddModelFromFile(
FindResource(file))
# Initialize the collision data
self.collision_frames = []
self.collision_poses = []
self.collision_radius = []
def Finalize(self):
"""
Cements the topology of the MultibodyPlant and identifies all available collision geometries.
"""
# Finalize the underlying plant model
self.multibody.Finalize()
# Idenify and store collision geometries
self.__store_collision_geometries()
def num_contacts(self):
""" returns the number of contact points"""
return len(self.collision_frames)
def num_friction(self):
""" returns the number of friction components"""
return 4 * self.dlevel * self.num_contacts()
def GetNormalDistances(self, context):
"""
Returns an array of signed distances between the contact geometries and the terrain, given the current system context
Arguments:
context: a pyDrake MultibodyPlant context
Return values:
distances: a numpy array of signed distance values
"""
qtype = self.multibody.GetPositions(context).dtype
nCollisions = len(self.collision_frames)
distances = np.zeros((nCollisions, ), dtype=qtype)
for n in range(0, nCollisions):
# Transform collision frames to world coordinates
collision_pt = self.multibody.CalcPointsPositions(
context, self.collision_frames[n],
self.collision_poses[n].translation(),
self.multibody.world_frame())
# Squeeze collision point (necessary for AutoDiff plants)
collision_pt = np.squeeze(collision_pt)
# Calc nearest point on terrain in world coordinates
terrain_pt = self.terrain.nearest_point(collision_pt)
# Calc normal distance to terrain
terrain_frame = self.terrain.local_frame(terrain_pt)
normal = terrain_frame[0, :]
distances[n] = normal.dot(collision_pt -
terrain_pt) - self.collision_radius[n]
# Return the distances as a single array
return distances
def GetContactJacobians(self, context):
"""
Returns a tuple of numpy arrays representing the normal and tangential contact Jacobians evaluated at each contact point
Arguments:
context: a pyDrake MultibodyPlant context
Return Values
(Jn, Jt): the tuple of contact Jacobians. Jn represents the normal components and Jt the tangential components
"""
qtype = self.multibody.GetPositions(context).dtype
numN = self.num_contacts()
numT = int(self.num_friction() / numN)
D = self.friction_discretization_matrix().transpose()
Jn = np.zeros((numN, self.multibody.num_velocities()), dtype=qtype)
Jt = np.zeros((numN * numT, self.multibody.num_velocities()),
dtype=qtype)
for n in range(0, numN):
# Transform collision frames to world coordinates
collision_pt = self.multibody.CalcPointsPositions(
context, self.collision_frames[n],
self.collision_poses[n].translation(),
self.multibody.world_frame())
# Calc nearest point on terrain in world coordinates
terrain_pt = self.terrain.nearest_point(collision_pt)
# Calc normal distance to terrain
terrain_frame = self.terrain.local_frame(terrain_pt)
normal, tangent = np.split(terrain_frame, [1], axis=0)
# Discretize to the chosen level
Dtangent = D.dot(tangent)
# Get the contact point Jacobian
J = self.multibody.CalcJacobianTranslationalVelocity(
context, JacobianWrtVariable.kV, self.collision_frames[n],
self.collision_poses[n].translation(),
self.multibody.world_frame(), self.multibody.world_frame())
# Calc contact Jacobians
Jn[n, :] = normal.dot(J)
Jt[n * numT:(n + 1) * numT, :] = Dtangent.dot(J)
# Return the Jacobians as a tuple of np arrays
return (Jn, Jt)
def GetFrictionCoefficients(self, context):
"""
Return friction coefficients for nearest point on terrain
Arguments:
context: the current MultibodyPlant context
Return Values:
friction_coeff: list of friction coefficients
"""
friction_coeff = []
for frame, pose in zip(self.collision_frames, self.collision_poses):
# Transform collision frames to world coordinates
collision_pt = self.multibody.CalcPointsPositions(
context, frame, pose.translation(),
self.multibody.world_frame())
# Calc nearest point on terrain in world coordiantes
terrain_pt = self.terrain.nearest_point(collision_pt)
friction_coeff.append(self.terrain.get_friction(terrain_pt))
# Return list of friction coefficients
return friction_coeff
def getTerrainPointsAndFrames(self, context):
"""
Return the nearest points on the terrain and the local coordinate frame
Arguments:
context: current MultibodyPlant context
Return Values:
terrain_pts: a 3xN array of points on the terrain
terrain_frames: a 3x3xN array, specifying the local frame of the terrain
"""
terrain_pts = []
terrain_frames = []
for frame, pose in zip(self.collision_frames, self.collision_poses):
# Calc collision point
collision_pt = self.multibody.CalcPointsPositions(
context, frame, pose.translation(),
self.multibody.world_frame())
# Calc nearest point on terrain in world coordinates
terrain_pt = self.terrain.nearest_point(collision_pt)
terrain_pts.append(terrain_pt)
# Calc local coordinate frame
terrain_frames.append(self.terrain.local_frame(terrain_pt))
return (terrain_pts, terrain_frames)
def toAutoDiffXd(self):
"""Covert the MultibodyPlant to use AutoDiffXd instead of Float"""
# Create a new TimeSteppingMultibodyPlant model
copy_ad = TimeSteppingMultibodyPlant(file=None,
terrain=self.terrain,
dlevel=self._dlevel)
# Instantiate the plant as the Autodiff version
copy_ad.multibody = self.multibody.ToAutoDiffXd()
copy_ad.scene_graph = self.scene_graph.ToAutoDiffXd()
copy_ad.model_index = self.model_index
# Store the collision frames to finalize the model
copy_ad.__store_collision_geometries()
return copy_ad
def set_discretization_level(self, dlevel=0):
"""Set the friction discretization level. The default is 0"""
self._dlevel = dlevel
def __store_collision_geometries(self):
"""Identifies the collision geometries in the model and stores their parent frame and pose in parent frame in lists"""
# Create a diagram and a scene graph
inspector = self.scene_graph.model_inspector()
# Locate collision geometries and contact points
body_inds = self.multibody.GetBodyIndices(self.model_index)
# Get the collision frames for each body in the model
for body_ind in body_inds:
body = self.multibody.get_body(body_ind)
collision_ids = self.multibody.GetCollisionGeometriesForBody(body)
for id in collision_ids:
# get and store the collision geometry frames
frame_name = inspector.GetName(
inspector.GetFrameId(id)).split("::")
self.collision_frames.append(
self.multibody.GetFrameByName(frame_name[-1]))
self.collision_poses.append(inspector.GetPoseInFrame(id))
# Check for a spherical geometry
geoms = inspector.GetGeometries(inspector.GetFrameId(id),
Role.kProximity)
shape = inspector.GetShape(geoms[0])
if type(shape) is Sphere:
self.collision_radius.append(shape.radius())
else:
self.collision_radius.append(0.)
def __discretize_friction(self, normal, tangent):
"""
Rotates the terrain tangent vectors to discretize the friction cone
Arguments:
normal: The terrain normal direction, (1x3) numpy array
tangent: The terrain tangent directions, (2x3) numpy array
Return Values:
all_tangents: The discretized friction vectors, (2nx3) numpy array
This method is now deprecated
"""
# Reflect the current friction basis
tangent = np.concatenate((tangent, -tangent), axis=0)
all_tangents = np.zeros((4 * (self._dlevel), tangent.shape[1]))
all_tangents[0:4, :] = tangent
# Rotate the tangent basis around the normal vector
for n in range(1, self._dlevel):
# Create an angle-axis representation of rotation
R = RotationMatrix(theta_lambda=AngleAxis(
angle=n * pi / (2 * (self._dlevel)), axis=normal))
# Apply the rotation matrix
all_tangents[n * 4:(n + 1) * 4, :] = R.multiply(
tangent.transpose()).transpose()
return all_tangents
def simulate(self, h, x0, u=None, N=1):
# Initialize arrays
nx = x0.shape[0]
x = np.zeros(shape=(nx, N))
x[:, 0] = x0
t = np.zeros(shape=(N, ))
nf = 1
if u is None:
B = self.multibody.MakeAcutatorMatrix()
u = np.zeros(shape=(B.shape[1], N))
context = self.multibody.CreateDefaultContext()
Jn, Jt = self.GetContactJacobians(context)
f = np.zeros(shape=(Jn.shape[0] + Jt.shape[0], N))
# Integration loop
for n in range(0, N - 1):
f[:, n] = self.contact_impulse(h, x[:, n], u[:, n])
x[:, n + 1] = self.integrate(h, x[:, n], u[:, n], f[:, n])
t[n + 1] = t[n] + h
f[:, n] = f[:, n] / h
return (t, x, f)
def integrate(self, h, x, u, f):
# Get the configuration and the velocity
q, dq = np.split(x, 2)
# Estimate the next configuration, assuming constant velocity
qhat = q + h * dq
# Set the context
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, qhat)
v = self.multibody.MapQDotToVelocity(context, dq)
self.multibody.SetVelocities(context, v)
# Get the current system properties
M = self.multibody.CalcMassMatrixViaInverseDynamics(context)
C = self.multibody.CalcBiasTerm(context)
G = self.multibody.CalcGravityGeneralizedForces(context)
B = self.multibody.MakeActuationMatrix()
Jn, Jt = self.GetContactJacobians(context)
J = np.vstack((Jn, Jt))
# Calculate the next state
b = h * (B.dot(u) - C.dot(dq) + G) + J.transpose().dot(f)
v = np.linalg.solve(M, b)
dq += v
q += h * dq
# Collect the configuration and velocity into a state vector
return np.concatenate((q, dq), axis=0)
def contact_impulse(self, h, x, u):
# Get the configuration and generalized velocity
q, dq = np.split(x, 2)
# Estimate the configuration at the next time step
qhat = q + h * dq
# Get the system parameters
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, q)
v = self.multibody.MapQDotToVelocity(context, dq)
self.multibody.SetVelocities(context, v)
M = self.multibody.CalcMassMatrixViaInverseDynamics(context)
C = self.multibody.CalcBiasTerm(context)
G = self.multibody.CalcGravityGeneralizedForces(context)
B = self.multibody.MakeActuationMatrix()
tau = B.dot(u) - C + G
Jn, Jt = self.GetContactJacobians(context)
phi = self.GetNormalDistances(context)
alpha = Jn.dot(qhat) - phi
# Calculate the force size from the contact Jacobian
numT = Jt.shape[0]
numN = Jn.shape[0]
S = numT + 2 * numN
# Initialize LCP parameters
P = np.zeros(shape=(S, S), dtype=float)
w = np.zeros(shape=(numN, S))
numF = int(numT / numN)
for n in range(0, numN):
w[n, n * numF + numN:numN + (n + 1) * numF] = 1
# Construct LCP matrix
J = np.vstack((Jn, Jt))
JM = J.dot(np.linalg.inv(M))
P[0:numN + numT, 0:numN + numT] = JM.dot(J.transpose())
P[:, numN + numT:] = w.transpose()
P[numN + numT:, :] = -w
P[numN + numT:,
0:numN] = np.diag(self.GetFrictionCoefficients(context))
#Construct LCP bias vector
z = np.zeros(shape=(S, ), dtype=float)
z[0:numN + numT] = h * JM.dot(tau) + J.dot(dq)
#z[0:numN] += (Jn.dot(q) - alpha)/h
z[0:numN] += phi / h
# Solve the LCP for the reaction impluses
f, status = solve_lcp(P, z)
if f is None:
return np.zeros(shape=(numN + numT, ))
else:
# Strip the slack variables from the LCP solution
return f[0:numN + numT]
def get_multibody(self):
return self.multibody
def joint_limit_jacobian(self):
"""
Returns a matrix for mapping the positive-only joint limit torques to
"""
# Create the Jacobian as if all joints were limited
nV = self.multibody.num_velocities()
I = np.eye(nV)
# Check for infinite limits
qhigh = self.multibody.GetPositionUpperLimits()
qlow = self.multibody.GetPositionLowerLimits()
# Assert two sided limits
low_inf = np.isinf(qlow)
assert all(low_inf == np.isinf(qhigh))
# Make the joint limit Jacobian
if not all(low_inf):
# Remove floating base limits
floating = self.multibody.GetFloatingBaseBodies()
floating_pos = []
while len(floating) > 0:
body = self.multibody.get_body(floating.pop())
if body.has_quaternion_dofs():
floating_pos.append(body.floating_positions_start())
# Remove entries corresponding to the extra dof from quaternions
low_inf = np.delete(low_inf, floating_pos)
low_inf = np.squeeze(low_inf)
# Zero-out the entries that don't correspond to joint limits
I[low_inf, low_inf] = 0
# Remove the columns that don't correspond to joint limits
I = np.delete(I, low_inf, axis=1)
return np.concatenate([I, -I], axis=1)
else:
return None
def get_contact_points(self, context):
"""
Returns a list of positions of contact points expressed in world coordinates given the system context.
"""
contact_pts = []
for frame, pose in zip(self.collision_frames, self.collision_poses):
contact_pts.append(
self.multibody.CalcPointsPositions(
context, frame, pose.translation(),
self.multibody.world_frame()))
return contact_pts
def resolve_contact_forces_in_world(self, context, forces):
"""
Transform non-negative discretized force components used in complementarity constraints into 3-vectors in world coordinates
Returns a list of (3,) numpy arrays
"""
# First remove the discretization
forces = self.resolve_forces(forces)
# Reorganize forces from (Normal, Tangential) to a list
force_list = []
for n in range(0, self.num_contacts()):
force_list.append(forces[[
n,
self.num_contacts() + 2 * n,
self.num_contacts() + 2 * n + 1
]])
# Transform the forces into world coordinates using the terrain frames
_, frames = self.getTerrainPointsAndFrames(context)
world_forces = []
for force, frame in zip(force_list, frames):
world_forces.append(frame.dot(force))
# Return a list of forces in world coordinates
return world_forces
def resolve_limit_forces(self, jl_forces):
"""
Combine positive and negative components of joint limit torques
Arguments:
jl_forces: (2n, m) numpy array
Return values:
(n, m) numpy array
"""
JL = self.joint_limit_jacobian()
has_limits = np.sum(abs(JL), axis=1) > 0
f_jl = JL.dot(jl_forces)
if f_jl.ndim > 1:
return f_jl[has_limits, :]
else:
return f_jl[has_limits]
def duplicator_matrix(self):
"""Returns a matrix of 1s and 0s for combining friction forces or duplicating sliding velocities"""
numN = self.num_contacts()
numT = self.num_friction()
w = np.zeros((numN, numT))
nD = int(numT / numN)
for n in range(numN):
w[n, n * nD:(n + 1) * nD] = 1
return w
def friction_discretization_matrix(self):
""" Make a matrix for converting discretized friction into a single vector"""
n = 4 * self.dlevel
theta = np.linspace(0, n - 1, n) * 2 * np.pi / n
D = np.vstack((np.cos(theta), np.sin(theta)))
# Threshold out small values
D[np.abs(D) < 1e-10] = 0
return D
def resolve_forces(self, forces):
""" Convert discretized friction & normal forces into a non-discretized 3-vector"""
numN = self.num_contacts()
n = 4 * self.dlevel
fN = forces[0:numN, :]
fT = forces[numN:numN * (n + 1), :]
D_ = self.friction_discretization_matrix()
D = np.zeros((2 * numN, n * numN))
for k in range(numN):
D[2 * k:2 * k + 2, k * n:(k + 1) * n] = D_
ff = D.dot(fT)
return np.concatenate((fN, ff), axis=0)
def static_controller(self, qref, verbose=False):
"""
Generates a controller to maintain a static pose
Arguments:
qref: (N,) numpy array, the static pose to be maintained
Return Values:
u: (M,) numpy array, actuations to best achieve static pose
f: (C,) numpy array, associated normal reaction forces
static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is:
N(q) = B*u + J*f
where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose.
Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported
"""
#TODO: Solve for friction forces as well
# Check inputs
if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions():
raise ValueError(
f"Reference position mut be ({self.multibody.num_positions(),},) array"
)
# Set the context
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, qref)
# Get the necessary properties
G = self.multibody.CalcGravityGeneralizedForces(context)
Jn, _ = self.GetContactJacobians(context)
phi = self.GetNormalDistances(context)
B = self.multibody.MakeActuationMatrix()
#Collect terms
A = np.concatenate([B, Jn.transpose()], axis=1)
# Create the mathematical program
prog = MathematicalProgram()
l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces")
u_var = prog.NewContinuousVariables(self.multibody.num_actuators(),
name="controls")
# Ensure dynamics approximately satisfied
prog.AddL2NormCost(A=A,
b=-G,
vars=np.concatenate([u_var, l_var], axis=0))
# Enforce normal complementarity
prog.AddBoundingBoxConstraint(np.zeros(l_var.shape),
np.full(l_var.shape, np.inf), l_var)
prog.AddConstraint(phi.dot(l_var) == 0)
# Solve
result = Solve(prog)
# Check for a solution
if result.is_success():
u = result.GetSolution(u_var)
f = result.GetSolution(l_var)
if verbose:
printProgramReport(result, prog)
return (u, f)
else:
print(f"Optimization failed. Returning zeros")
if verbose:
printProgramReport(result, prog)
return (np.zeros(u_var.shape), np.zeros(l_var.shape))
@property
def dlevel(self):
return self._dlevel
@dlevel.setter
def dlevel(self, val):
"""Check that the value of dlevel is a positive integer"""
if type(val) is int and val > 0:
self._dlevel = val
else:
raise ValueError("dlevel must be a positive integer")
parser = argparse.ArgumentParser()
parser.add_argument("-g",
"--gravity",
type=float,
help="Gravitational constant (m/s^2).",
default=9.8)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
controller = builder.AddSystem(Controller(plant, args.gravity))
builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8,
2.8]))
# Build
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization and env in MeshCat
from pydrake.geometry import Box
from pydrake.common.eigen_geometry import Isometry3
from pydrake.all import AddMultibodyPlantSceneGraph
from pydrake.multibody.tree import SpatialInertia, UnitInertia
env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
for idx, object_ in enumerate(OBJECTS):
sz, trans, rot = object_
body = env_plant.AddRigidBody(
str(idx + 1),
SpatialInertia(1.0, np.zeros(3), UnitInertia(1.0, 1.0, 1.0)))
pos = Isometry3()
pos.set_translation(trans) # np.array([0,1,1])
pos.set_rotation(rot) # rm([1,0,0], 90)
env_plant.RegisterVisualGeometry(body, pos, Box(sz[0], sz[1], sz[2]),
str(idx + 1) + 'v',
np.array([1, 1, 1, 1]), scene_graph)
env_plant.Finalize()
plant.RegisterGeometry(scene_graph)
builder.Connect(plant.get_geometry_pose_output_port(),
scene_graph.get_source_pose_port(plant.source_id()))
def perform_iou_testing(model_file, test_specific_temp_directory,
pose_directory):
random_poses = {}
# Read camera translation calculated and applied on gazebo
# we read the random positions file as it contains everything:
with open(
test_specific_temp_directory + "/pics/" + pose_directory +
"/poses.txt", "r") as datafile:
for line in datafile:
if line.startswith("Translation:"):
line_split = line.split(" ")
# we make the value negative since gazebo moved the robot
# and in drakewe move the camera
trans_x = float(line_split[1])
trans_y = float(line_split[2])
trans_z = float(line_split[3])
elif line.startswith("Scaling:"):
line_split = line.split(" ")
scaling = float(line_split[1])
else:
line_split = line.split(" ")
if line_split[1] == "nan":
random_poses[line_split[0][:-1]] = 0
else:
random_poses[line_split[0][:-1]] = float(line_split[1])
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
model = make_parser(plant).AddModelFromFile(model_file)
model_bodies = me.get_bodies(plant, {model})
frame_W = plant.world_frame()
frame_B = model_bodies[0].body_frame()
if len(plant.GetBodiesWeldedTo(plant.world_body())) < 2:
plant.WeldFrames(frame_W,
frame_B,
X_PC=plant.GetDefaultFreeBodyPose(frame_B.body()))
# Creating cameras:
renderer_name = "renderer"
scene_graph.AddRenderer(renderer_name,
MakeRenderEngineVtk(RenderEngineVtkParams()))
# N.B. These properties are chosen arbitrarily.
depth_camera = DepthRenderCamera(
RenderCameraCore(
renderer_name,
CameraInfo(
width=960,
height=540,
focal_x=831.382036787,
focal_y=831.382036787,
center_x=480,
center_y=270,
),
ClippingRange(0.01, 10.0),
RigidTransform(),
),
DepthRange(0.01, 10.0),
)
world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index())
# Creating perspective cam
X_WB = xyz_rpy_deg(
[
1.6 / scaling + trans_x, -1.6 / scaling + trans_y,
1.2 / scaling + trans_z
],
[-120, 0, 45],
)
sensor_perspective = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating top cam
X_WB = xyz_rpy_deg([0 + trans_x, 0 + trans_y, 2.2 / scaling + trans_z],
[-180, 0, -90])
sensor_top = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating front cam
X_WB = xyz_rpy_deg([2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z],
[-90, 0, 90])
sensor_front = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating side cam
X_WB = xyz_rpy_deg([0 + trans_x, 2.2 / scaling + trans_y, 0 + trans_z],
[-90, 0, 180])
sensor_side = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating back cam
X_WB = xyz_rpy_deg([-2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z],
[-90, 0, -90])
sensor_back = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Remove gravity to avoid extra movements of the model when running the simulation
plant.gravity_field().set_gravity_vector(
np.array([0, 0, 0], dtype=np.float64))
# Switch off collisions to avoid problems with random positions
collision_filter_manager = scene_graph.collision_filter_manager()
model_inspector = scene_graph.model_inspector()
geometry_ids = GeometrySet(model_inspector.GetAllGeometryIds())
collision_filter_manager.Apply(
CollisionFilterDeclaration().ExcludeWithin(geometry_ids))
plant.Finalize()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
dofs = plant.num_actuated_dofs()
if dofs != plant.num_positions():
raise ValueError(
"Error on converted model: Num positions is not equal to num actuated dofs."
)
if pose_directory == "random_pose":
joint_positions = [0] * dofs
for joint_name, pose in random_poses.items():
# check if NaN
if pose != pose:
pose = 0
# drake will add '_joint' when there's a name collision
if plant.HasJointNamed(joint_name):
joint = plant.GetJointByName(joint_name)
else:
joint = plant.GetJointByName(joint_name + "_joint")
joint_positions[joint.position_start()] = pose
sim_plant_context = plant.GetMyContextFromRoot(
simulator.get_mutable_context())
plant.get_actuation_input_port(model).FixValue(sim_plant_context,
np.zeros((dofs, 1)))
plant.SetPositions(sim_plant_context, model, joint_positions)
simulator.AdvanceTo(1)
generate_images_and_iou(simulator, sensor_perspective,
test_specific_temp_directory, pose_directory, 1)
generate_images_and_iou(simulator, sensor_top,
test_specific_temp_directory, pose_directory, 2)
generate_images_and_iou(simulator, sensor_front,
test_specific_temp_directory, pose_directory, 3)
generate_images_and_iou(simulator, sensor_side,
test_specific_temp_directory, pose_directory, 4)
generate_images_and_iou(simulator, sensor_back,
test_specific_temp_directory, pose_directory, 5)
class iiwa_sys():
def __init__(self,
builder,
dt=5e-4,
N=150,
params=None,
trj_decay=0.7,
x_w_cov=1e-5,
door_angle_ref=1.0,
visualize=False):
self.plant_derivs = MultibodyPlant(time_step=dt)
parser = Parser(self.plant_derivs)
self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
parser,
params=params)
self.plant_derivs.Finalize()
self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
self.plant_derivs.get_actuation_input_port().FixValue(
self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
self.plant_derivs_context, null_force)
self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=dt)
parser = Parser(self.plant, scene_graph)
self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
parser,
params=params)
self.plant.Finalize(
) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.
self.meshcat = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=zmq_url)
self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff
self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls
nq = self.plant.num_positions()
nv = self.plant.num_velocities()
self.n_x = nq + nv
self.n_u = self.plant.num_actuators()
self.n_y = self.plant.get_state_output_port(self.iiwa).size()
self.N = N
self.dt = dt
self.decay = trj_decay
self.V = 1e-2 * np.ones(self.n_y)
self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
self.x_w_cov = x_w_cov
self.door_angle_ref = door_angle_ref
self.q0 = np.array(
[-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
#self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
self.x0 = np.concatenate((self.q0, np.zeros(nv)))
self.door_index = None
self.phi = {}
def ports_init(self, context):
self.plant_context = self.plant.GetMyMutableContextFromRoot(context)
self.plant.SetPositionsAndVelocities(self.plant_context, self.x0)
#door_angle = self.plant.GetPositionsFromArray(self.hinge, self.x0[:8])
self.door_index = self.plant.GetJointByName(
'right_door_hinge').position_start()
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant.GetInputPort("applied_generalized_force").FixValue(
self.plant_context, null_force)
self.plant.get_actuation_input_port().FixValue(
self.plant_context, [0., 0., 0., 0., 0., 0., 0.])
self.W0[self.door_index] = self.x_w_cov
def add_models(self, plant, parser, params=None):
iiwa = parser.AddModelFromFile(
"/home/hanikevi/drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision_no_grav.sdf"
)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
#box = Box(10., 10., 10.)
#X_WBox = RigidTransform([0, 0, -5])
#mu = 0.6
#plant.RegisterCollisionGeometry(plant.world_body(), X_WBox, box, "ground", CoulombFriction(mu, mu))
#plant.RegisterVisualGeometry(plant.world_body(), X_WBox, box, "ground", [.9, .9, .9, 1.0])
#planar_joint_frame = plant.AddFrame(FixedOffsetFrame("planar_joint_frame", plant.world_frame(), RigidTransform(RotationMatrix.MakeXRotation(np.pi/2))))
X_WCylinder = RigidTransform([-0.75, 0, 0.5])
hinge = parser.AddModelFromFile(
"/home/hanikevi/drake/examples/manipulation_station/models/simple_hinge.sdf"
)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"),
X_WCylinder)
#cupboard_door_spring = plant.AddForceElement(RevoluteSpring_[float](plant.GetJointByName("right_door_hinge"), nominal_angle = -0.4, stiffness = 10))
if params is None:
bushing = LinearBushingRollPitchYaw_[float](
plant.GetFrameByName("iiwa_link_7"),
plant.GetFrameByName("handle"),
[50, 50, 50], # Torque stiffness
[2., 2., 2.], # Torque damping
[5e4, 5e4, 5e4], # Linear stiffness
[80, 80, 80], # Linear damping
)
else:
print('setting custom stiffnesses')
bushing = LinearBushingRollPitchYaw_[float](
plant.GetFrameByName("iiwa_link_7"),
plant.GetFrameByName("handle"),
[params['k4'], params['k5'], params['k6']], # Torque stiffness
[2, 2, 2], # Torque damping
[params['k1'], params['k2'], params['k3']], # Linear stiffness
[100, 100, 100], # Linear damping
)
bushing_element = plant.AddForceElement(bushing)
return iiwa, hinge, bushing
def cost_stage(self, x, u):
ctrl = 1e-5 * np.sum(u**2)
pos = 15.0 * (x[self.door_index] - self.door_angle_ref)**2
vel = 1e-5 * np.sum(x[8:]**2)
return pos + ctrl + vel
def cost_final(self, x):
return 50 * (1.0 * (x[self.door_index] - self.door_angle_ref)**2 +
np.sum(2.5e-4 * x[8:]**2))
def get_deriv(self, x, u):
self.plant_derivs.SetPositionsAndVelocities(self.plant_derivs_context,
x)
lin = FirstOrderTaylorApproximation(
self.plant_derivs, self.plant_derivs_context,
self.plant.get_actuation_input_port().get_index(),
self.plant.get_state_output_port(self.iiwa).get_index())
return lin.A(), lin.B(), lin.C()
def get_param_deriv(self, x, u):
# Using a closed-form solution; as currently DRAKE doesn't do support autodiff on parameters for LinearBusing.
# Note dC is 0 - stiffness does not affect measurements of q, v
W = self.plant.world_frame()
I = self.plant.GetFrameByName("iiwa_link_7")
H = self.plant.GetFrameByName("handle")
self.plant.SetPositionsAndVelocities(self.plant_context, x)
M = self.plant.CalcMassMatrixViaInverseDynamics(self.plant_context)
Jac_I = self.plant.CalcJacobianSpatialVelocity(
self.plant_context, JacobianWrtVariable.kQDot, I, [0, 0, 0], W, W)
Jac_H = self.plant.CalcJacobianSpatialVelocity(
self.plant_context, JacobianWrtVariable.kQDot, H, [0, 0, 0], W, W)
dA = np.zeros((self.n_x**2, 3))
dA_sq = np.zeros((self.n_x, self.n_x))
for param_ind in range(3):
JH = np.outer(Jac_H[param_ind, :], Jac_H[param_ind, :])
JI = np.outer(Jac_I[param_ind, :], Jac_I[param_ind, :])
dA_sq[8:, :8] = self.dt * np.linalg.inv(M).dot(JH + JI)
dA[:, param_ind] = deepcopy(dA_sq.ravel())
#print(np.sum(np.abs(dA), axis=0))
return dA
def reset(self):
x_traj_new = np.zeros((self.N + 1, self.n_x))
x_traj_new[0, :] = self.x0 + np.multiply(np.sqrt(self.W0),
np.random.randn(self.n_x))
u_traj_new = np.zeros((self.N, self.n_u))
#self.plant_context.SetDiscreteState(x_traj_new[0,:])
self.plant.SetPositionsAndVelocities(self.plant_context,
x_traj_new[0, :])
return x_traj_new, u_traj_new
def rollout(self):
self.u_trj = np.random.randn(self.N, self.n_u) * 0.001
self.x_trj, _ = self.reset()
for i in range(self.N):
self.x_trj[i + 1, :] = self.dyn(self.x_trj[i, :],
self.u_trj[i],
noise=True)
def dyn(self, x, u, phi=None, noise=False):
x_next = self.plant.AllocateDiscreteVariables()
self.plant.SetPositionsAndVelocities(self.plant_context, x)
self.plant.get_actuation_input_port(self.iiwa).FixValue(
self.plant_context, u)
self.plant.CalcDiscreteVariableUpdates(self.plant_context, x_next)
#print(x_next.get_mutable_vector().get_value())
x_new = x_next.get_mutable_vector().get_value()
if noise:
noise = np.multiply(np.sqrt(self.W), np.random.randn(self.n_x))
x_new += noise
return x_new
def obs(self, x, mode=None, noise=False, phi=None):
y = self.plant.get_state_output_port(self.iiwa).Eval(
self.plant_context)
#print(self.bushing.CalcBushingSpatialForceOnFrameA(self.plant_context).translational())
if noise:
y += np.multiply(np.sqrt(self.V), np.random.randn(self.n_y))
return y
def cost(self, x_trj=None, u_trj=None):
cost_trj = 0.0
if x_trj is None:
for i in range(self.N):
cost_trj += self.cost_stage(self.x_trj[i, :], self.u_trj[i, :])
cost_trj += self.cost_final(self.sys.plant, self.x_trj[-1, :])
else:
for i in range(self.N):
cost_trj += self.cost_stage(x_trj[i, :], u_trj[i, :])
cost_trj += self.cost_final(x_trj[-1, :])
return cost_trj
def compile_scene_tree_to_mbp_and_sg(scene_tree, timestep=0.001):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=timestep))
parser = Parser(mbp)
parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH")
world_body = mbp.world_body()
node_to_free_body_ids_map = {}
body_id_to_node_map = {}
free_body_poses = []
for node in scene_tree.nodes:
node_to_free_body_ids_map[node] = []
if node.tf is not None and node.physics_geometry_info is not None:
# Don't have to do anything if this does not introduce geometry.
sanity_check_node_tf_and_physics_geom_info(node)
phys_geom_info = node.physics_geometry_info
# Don't have to do anything if this does not introduce geometry.
has_models = len(phys_geom_info.model_paths) > 0
has_prim_geometry = (len(phys_geom_info.visual_geometry) +
len(phys_geom_info.collision_geometry)) > 0
if not has_models and not has_prim_geometry:
continue
node_model_ids = []
# Handle adding primitive geometry by adding it all to one
# mbp.
if has_prim_geometry:
# Contain this primitive geometry in a model instance.
model_id = mbp.AddModelInstance(node.name + "::model_%d" %
len(node_model_ids))
node_model_ids.append(model_id)
# Add a body for this node, and register any of the
# visual and collision geometry available.
# TODO(gizatt) This tree body index is built in to disambiguate names.
# But I forsee a name-to-stuff resolution crisis when inference time comes...
# this might get resolved by the solution to that.
body = mbp.AddRigidBody(name=node.name,
model_instance=model_id,
M_BBo_B=phys_geom_info.spatial_inertia)
body_id_to_node_map[body.index()] = node
tf = torch_tf_to_drake_tf(node.tf)
if phys_geom_info.fixed:
weld = mbp.WeldFrames(world_body.body_frame(),
body.body_frame(), tf)
else:
node_to_free_body_ids_map[node].append(body.index())
mbp.SetDefaultFreeBodyPose(body, tf)
for k, (tf, geometry,
color) in enumerate(phys_geom_info.visual_geometry):
mbp.RegisterVisualGeometry(body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name +
"_vis_%03d" % k,
diffuse_color=color)
for k, (tf, geometry, friction) in enumerate(
phys_geom_info.collision_geometry):
mbp.RegisterCollisionGeometry(
body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name + "_col_%03d" % k,
coulomb_friction=friction)
# Handle adding each model from sdf/urdf.
if has_models:
for local_tf, model_path, root_body_name, q0_dict in phys_geom_info.model_paths:
model_id = parser.AddModelFromFile(
resolve_catkin_package_path(parser.package_map(),
model_path),
node.name + "::"
"model_%d" % len(node_model_ids))
node_model_ids.append(model_id)
if root_body_name is None:
root_body_ind_possibilities = mbp.GetBodyIndices(
model_id)
assert len(root_body_ind_possibilities) == 1, \
"Please supply root_body_name for model with path %s" % model_path
root_body = mbp.get_body(
root_body_ind_possibilities[0])
else:
root_body = mbp.GetBodyByName(name=root_body_name,
model_instance=model_id)
body_id_to_node_map[root_body.index()] = node
node_tf = torch_tf_to_drake_tf(node.tf)
full_model_tf = node_tf.multiply(
torch_tf_to_drake_tf(local_tf))
if phys_geom_info.fixed:
mbp.WeldFrames(world_body.body_frame(),
root_body.body_frame(), full_model_tf)
else:
node_to_free_body_ids_map[node].append(
root_body.index())
mbp.SetDefaultFreeBodyPose(root_body, full_model_tf)
# Handle initial joint state
if q0_dict is not None:
for joint_name in list(q0_dict.keys()):
q0_this = q0_dict[joint_name]
joint = mbp.GetMutableJointByName(
joint_name, model_instance=model_id)
# Reshape to make Drake happy.
q0_this = q0_this.reshape(joint.num_positions(), 1)
joint.set_default_positions(q0_this)
return builder, mbp, scene_graph, node_to_free_body_ids_map, body_id_to_node_map
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
Simulator, UniformGravityFieldElement)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8,
2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
context.FixInputPort(0, [0., 0.]) # Zero input torques
R = [1]
# MultibodyPlant has many (optional) input ports, so we must pass the
# input_port_index to LQR.
return LinearQuadraticRegulator(
plant,
context,
Q,
R,
input_port_index=plant.get_actuation_input_port().get_index())
if __name__ == "__main__":
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
controller = builder.AddSystem(BalancingLQR(plant))
builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.5, 2.5],
ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
# PyDrake Imports
from pydrake.common import FindResourceOrThrow
from pydrake.all import MultibodyPlant, DiagramBuilder, AddMultibodyPlantSceneGraph, RigidTransform, JacobianWrtVariable
from pydrake.multibody.parsing import Parser
# Import the model from a URDF file
model_file = "Systems/urdf/fallingBox.urdf"
if not path.isfile(model_file):
exit(f"Path {model_file} not found")
else:
model_file = path.abspath(model_file)
print(model_file)
# Set up the RigidBodyPlant model in Drake - Note that without the DiagramBuilder and SceneGraph, pydrake return that there are no collision geometries
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
box = Parser(plant).AddModelFromFile(model_file)
body_inds = plant.GetBodyIndices(box)
base_frame = plant.get_body(body_inds[0]).body_frame()
world_frame = plant.world_frame()
plant.WeldFrames(world_frame, base_frame, RigidTransform())
plant.Finalize()
context = plant.CreateDefaultContext()
print(f"fallingbox has {plant.num_positions()} position coordinates")
# We will also need a scene graph inspector
inspector = scene_graph.model_inspector()
# Based on the URDF, Box should have 8 collision geometries
print(f"Box has {plant.num_collision_geometries()} collision geometries")
# Locate the collision geometries and contact points
# First get the rigid body elements
collisionIds = []
def make_box_flipup(generator,
observations="state",
meshcat=None,
time_limit=10):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
# TODO(russt): randomize parameters.
box = AddPlanarBinAndSimpleBox(plant)
finger = AddPointFinger(plant)
plant.Finalize()
plant.set_name("plant")
SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id())
controller_plant = MultibodyPlant(time_step=0.005)
AddPointFinger(controller_plant)
if meshcat:
MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3)
ContactVisualizer.AddToBuilder(
builder, plant, meshcat,
ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0))
# Use the controller plant to visualize the set point geometry.
controller_scene_graph = builder.AddSystem(SceneGraph())
controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph)
SetColor(controller_scene_graph,
color=[1.0, 165.0 / 255, 0.0, 1.0],
source_id=controller_plant.get_source_id())
controller_vis = MeshcatVisualizerCpp.AddToBuilder(
builder, controller_scene_graph, meshcat,
MeshcatVisualizerParams(prefix="controller"))
controller_vis.set_name("controller meshcat")
controller_plant.Finalize()
# Stiffness control. (For a point finger with unit mass, the
# InverseDynamicsController is identical)
N = controller_plant.num_positions()
kp = [100] * N
ki = [1] * N
kd = [2 * np.sqrt(kp[0])] * N
controller = builder.AddSystem(
InverseDynamicsController(controller_plant, kp, ki, kd, False))
builder.Connect(plant.get_state_output_port(finger),
controller.get_input_port_estimated_state())
actions = builder.AddSystem(PassThrough(N))
positions_to_state = builder.AddSystem(Multiplexer([N, N]))
builder.Connect(actions.get_output_port(),
positions_to_state.get_input_port(0))
zeros = builder.AddSystem(ConstantVectorSource([0] * N))
builder.Connect(zeros.get_output_port(),
positions_to_state.get_input_port(1))
builder.Connect(positions_to_state.get_output_port(),
controller.get_input_port_desired_state())
builder.Connect(controller.get_output_port(),
plant.get_actuation_input_port())
if meshcat:
positions_to_poses = builder.AddSystem(
MultibodyPositionToGeometryPose(controller_plant))
builder.Connect(
positions_to_poses.get_output_port(),
controller_scene_graph.get_source_pose_port(
controller_plant.get_source_id()))
builder.ExportInput(actions.get_input_port(), "actions")
if observations == "state":
builder.ExportOutput(plant.get_state_output_port(), "observations")
# TODO(russt): Add 'time', and 'keypoints'
else:
raise ValueError("observations must be one of ['state']")
class RewardSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("box_state", 6)
self.DeclareVectorInputPort("finger_state", 4)
self.DeclareVectorInputPort("actions", 2)
self.DeclareVectorOutputPort("reward", 1, self.CalcReward)
def CalcReward(self, context, output):
box_state = self.get_input_port(0).Eval(context)
finger_state = self.get_input_port(1).Eval(context)
actions = self.get_input_port(2).Eval(context)
angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2
cost = 2 * angle_from_vertical**2 # box angle
cost += 0.1 * box_state[5]**2 # box velocity
effort = actions - finger_state[:2]
cost += 0.1 * effort.dot(effort) # effort
# finger velocity
cost += 0.1 * finger_state[2:].dot(finger_state[2:])
# Add 10 to make rewards positive (to avoid rewarding simulator
# crashes).
output[0] = 10 - cost
reward = builder.AddSystem(RewardSystem())
builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0))
builder.Connect(plant.get_state_output_port(finger),
reward.get_input_port(1))
builder.Connect(actions.get_output_port(), reward.get_input_port(2))
builder.ExportOutput(reward.get_output_port(), "reward")
# Set random state distributions.
uniform_random = Variable(name="uniform_random",
type=Variable.Type.RANDOM_UNIFORM)
box_joint = plant.GetJointByName("box")
x, y = box_joint.get_default_translation()
box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0)
diagram = builder.Build()
simulator = Simulator(diagram)
# Termination conditions:
def monitor(context):
if context.get_time() > time_limit:
return EventStatus.ReachedTermination(diagram, "time limit")
return EventStatus.Succeeded()
simulator.set_monitor(monitor)
return simulator
"""First we set a couple of numeric parameters."""
# unstable equilibrium point
x_star = [0, np.pi, 0, 0]
# weight matrices for the lqr controller
Q = np.eye(4)
R = np.eye(1)
"""Then we construct the block diagram with the cart-pole in closed loop with the LQR controller."""
# start construction site of our block diagram
builder = DiagramBuilder()
# instantiate the cart-pole and the scene graph
cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(cartpole).AddModelFromFile(urdf_path)
cartpole.Finalize()
# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)
# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
import argparse
import numpy as np
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
PlanarSceneGraphVisualizer, Simulator)
from underactuated import FindResource, SliderSystem
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
plant.get_actuation_input_port())
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True):
# Make some random lumpy objects
trimeshes = []
np.random.seed(42)
for k in range(3):
# Make a small random number of triangles and chull it
# to get a lumpy object
mesh = trimesh.creation.random_soup(5)
mesh = trimesh.convex.convex_hull(mesh)
trimeshes.append(mesh)
# Create Drake geometry from those objects by adding a small
# sphere at each vertex
sphere_rad = 0.05
cmap = plt.cm.get_cmap('jet')
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.001))
# Add ground
friction = CoulombFriction(0.9, 0.8)
g = pydrake_geom.Box(100., 100., 0.5)
tf = RigidTransform(p=[0., 0., -0.25])
mbp.RegisterVisualGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
diffuse_color=[1.0, 1.0, 1.0, 1.0])
mbp.RegisterCollisionGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
coulomb_friction=friction)
for i, mesh in enumerate(trimeshes):
inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia)
color = cmap(np.random.random())
if verts_geom:
for j, vert in enumerate(mesh.vertices):
g = pydrake_geom.Sphere(radius=sphere_rad)
tf = RigidTransform(p=vert)
mbp.RegisterVisualGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_color_%d" % (i, j),
diffuse_color=color)
mbp.RegisterCollisionGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_collision_%d" %
(i, j),
coulomb_friction=friction)
# And add mesh itself for vis
path = "/tmp/part_%d.obj" % i
trimesh.exchange.export.export_mesh(mesh, path)
g = pydrake_geom.Convex(path)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base" % i,
diffuse_color=color)
if convex_collision_geom:
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base_col" % i,
coulomb_friction=friction)
mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.]))
mbp.Finalize()
return builder, mbp, scene_graph
import argparse
import pydrake
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Meshcat,
MeshcatVisualizerParams, MeshcatVisualizerCpp, Parser,
Role, Simulator, MultibodyPlant)
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description='Do interactive placement of objects.')
parser.add_argument('model_path', help='Path to model SDF/URDF.')
args = parser.parse_args()
# Build MBP
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=1E-3))
# Parse requested file
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(args.model_path)
mbp.Finalize()
# Visualizer
meshcat = Meshcat()
vis = MeshcatVisualizerCpp.AddToBuilder(builder,
scene_graph,
meshcat=meshcat)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetSubsystemContext(mbp, diagram_context)
import numpy as np
from pydrake.all import (DiagramBuilder, AddMultibodyPlantSceneGraph,
ConnectPlanarSceneGraphVisualizer, Parser,
PiecewisePolynomial)
from manipulation.utils import FindResource
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(
FindResource("models/double_pendulum.urdf"))
plant.Finalize()
viz = ConnectPlanarSceneGraphVisualizer(builder,
scene_graph,
show=False,
xlim=[-.2, 2.2],
ylim=[-1., 1.])
viz.fig.set_size_inches([3, 2.5])
diagram = builder.Build()
context = diagram.CreateDefaultContext()
T = 2.
q = PiecewisePolynomial.FirstOrderHold(
[0, T], np.array([[-np.pi / 2.0 + 1., -np.pi / 2.0 - 1.], [-2., 2.]]))
plant_context = plant.GetMyContextFromRoot(context)
viz.start_recording()
for t in np.linspace(0, T, num=100):
context.SetTime(t)
from pydrake.all import (AddMultibodyPlantSceneGraph,
DiagramBuilder,
Parser,
Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
-kp * q[1] - kd * v[1]])
parser = argparse.ArgumentParser()
parser.add_argument("-g", "--gravity",
type=float,
help="Gravitational constant (m/s^2).",
default=9.8)
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
controller = builder.AddSystem(Controller(plant, args.gravity))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
def MakeManipulationStation(time_step=0.002,
plant_setup_callback=None,
camera_prefix="camera"):
"""
Sets up a manipulation station with the iiwa + wsg + controllers [+
cameras]. Cameras will be added to each body with a name starting with
"camera".
Args:
time_step: the standard MultibodyPlant time step.
plant_setup_callback: should be a python callable that takes one
argument: `plant_setup_callback(plant)`. It will be called after the
iiwa and WSG are added to the plant, but before the plant is
finalized. Use this to add additional geometry.
camera_prefix: Any bodies in the plant (created during the
plant_setup_callback) starting with this prefix will get a camera
attached.
"""
builder = DiagramBuilder()
# Add (only) the iiwa, WSG, and cameras to the scene.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=time_step)
iiwa = AddIiwa(plant)
wsg = AddWsg(plant, iiwa)
if plant_setup_callback:
plant_setup_callback(plant)
plant.Finalize()
num_iiwa_positions = plant.num_positions(iiwa)
# I need a PassThrough system so that I can export the input port.
iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions))
builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position")
builder.ExportOutput(iiwa_position.get_output_port(),
"iiwa_position_command")
# Export the iiwa "state" outputs.
demux = builder.AddSystem(
Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions))
builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port())
builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured")
builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated")
builder.ExportOutput(plant.get_state_output_port(iiwa),
"iiwa_state_estimated")
# Make the plant for the iiwa controller to use.
controller_plant = MultibodyPlant(time_step=time_step)
controller_iiwa = AddIiwa(controller_plant)
AddWsg(controller_plant, controller_iiwa, welded=True)
controller_plant.Finalize()
# Add the iiwa controller
iiwa_controller = builder.AddSystem(
InverseDynamicsController(controller_plant,
kp=[100] * num_iiwa_positions,
ki=[1] * num_iiwa_positions,
kd=[20] * num_iiwa_positions,
has_reference_acceleration=False))
iiwa_controller.set_name("iiwa_controller")
builder.Connect(plant.get_state_output_port(iiwa),
iiwa_controller.get_input_port_estimated_state())
# Add in the feed-forward torque
adder = builder.AddSystem(Adder(2, num_iiwa_positions))
builder.Connect(iiwa_controller.get_output_port_control(),
adder.get_input_port(0))
# Use a PassThrough to make the port optional (it will provide zero values
# if not connected).
torque_passthrough = builder.AddSystem(PassThrough([0]
* num_iiwa_positions))
builder.Connect(torque_passthrough.get_output_port(),
adder.get_input_port(1))
builder.ExportInput(torque_passthrough.get_input_port(),
"iiwa_feedforward_torque")
builder.Connect(adder.get_output_port(),
plant.get_actuation_input_port(iiwa))
# Add discrete derivative to command velocities.
desired_state_from_position = builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
num_iiwa_positions, time_step, suppress_initial_transient=True))
desired_state_from_position.set_name("desired_state_from_position")
builder.Connect(desired_state_from_position.get_output_port(),
iiwa_controller.get_input_port_desired_state())
builder.Connect(iiwa_position.get_output_port(),
desired_state_from_position.get_input_port())
# Export commanded torques.
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded")
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured")
builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa),
"iiwa_torque_external")
# Wsg controller.
wsg_controller = builder.AddSystem(SchunkWsgPositionController())
wsg_controller.set_name("wsg_controller")
builder.Connect(wsg_controller.get_generalized_force_output_port(),
plant.get_actuation_input_port(wsg))
builder.Connect(plant.get_state_output_port(wsg),
wsg_controller.get_state_input_port())
builder.ExportInput(wsg_controller.get_desired_position_input_port(),
"wsg_position")
builder.ExportInput(wsg_controller.get_force_limit_input_port(),
"wsg_force_limit")
wsg_mbp_state_to_wsg_state = builder.AddSystem(
MakeMultibodyStateToWsgStateSystem())
builder.Connect(plant.get_state_output_port(wsg),
wsg_mbp_state_to_wsg_state.get_input_port())
builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(),
"wsg_state_measured")
builder.ExportOutput(wsg_controller.get_grip_force_output_port(),
"wsg_force_measured")
# Cameras.
AddRgbdSensors(builder,
plant,
scene_graph,
model_instance_prefix=camera_prefix)
# Export "cheat" ports.
builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query")
builder.ExportOutput(plant.get_contact_results_output_port(),
"contact_results")
builder.ExportOutput(plant.get_state_output_port(),
"plant_continuous_state")
builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses")
diagram = builder.Build()
diagram.set_name("ManipulationStation")
return diagram
-kp * q[1] - kd * v[1]])
parser = argparse.ArgumentParser()
parser.add_argument("-g", "--gravity",
type=float,
help="Gravitational constant (m/s^2).",
default=9.8)
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
controller = builder.AddSystem(Controller(plant, args.gravity))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
