def add_ground(plant):
ground_model = plant.AddModelInstance("ground_model")
ground_box = plant.AddRigidBody(
"ground", ground_model,
SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
X_WG = RigidTransform([0, 0, -0.05])
ground_geometry_id = plant.RegisterCollisionGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
CoulombFriction(0.3, 0.25))
plant.RegisterVisualGeometry(ground_box, RigidTransform(),
Box(10, 10,
0.1), "ground", [0.5, 0.5, 0.5, 1.])
plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
def __init__(self, name, tf, height, width):
PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
# Handle geometry and physics.
self.wall_thickness = 0.1
self.width = width
self.height = height
# Move the collision geometry so the wall surface is at y=0 (local frame),
# and is open in the -y direction, and the base of the wall is at z=0.
geom_tf = pose_to_tf_matrix(
torch.tensor(
[0., self.wall_thickness / 2., height / 2., 0., 0., 0.]))
geometry = Box(width=width, depth=self.wall_thickness, height=height)
self.register_geometry(geom_tf,
geometry,
color=np.array([1., 0.898, 0.706, 1.0]))
# Use the same geom as clearance geometry
self.register_clearance_geometry(geom_tf, geometry)
# This node produces a geometric number of cabinets on its surface.
cabinet_production_rule = RandomRelativePoseProductionRule(
Cabinet, "%s_cabinet" % name, self._sample_cabinet_pose_on_wall)
GeometricSetNode.__init__(self,
name=name,
production_rule=cabinet_production_rule,
geometric_prob=0.5)
def __init__(self, name, tf, width, length):
PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
floor_depth = 0.1
# Move the collision geometry so the surface is at z=0
geom_tf = pose_to_tf_matrix(
torch.tensor([0., 0., -floor_depth / 2., 0., 0., 0.]))
geometry = Box(width=width, depth=length, height=floor_depth)
self.register_geometry(geom_tf,
geometry,
color=np.array([0.8, 0.8, 0.8, 1.0]))
# Spawn a table at a determined location.
# (Currently just for testing item placement.)
table_spawn_rule = DeterministicRelativePoseProductionRule(
child_constructor=Table,
child_name="%s_table" % name,
relative_tf=pose_to_tf_matrix(
torch.tensor([1., 0., 0., 0., 0., 0.])))
robot_spawn_rule = DeterministicRelativePoseProductionRule(
child_constructor=RobotSpawnLocation,
child_name="%s_robot_spawn" % name,
relative_tf=pose_to_tf_matrix(
torch.tensor([-1, 0., 0., 0., 0., 0.])))
AndNode.__init__(self,
name=name,
production_rules=[table_spawn_rule, robot_spawn_rule])
def AddPlanarBinAndSimpleBox(plant,
mass=1.0,
mu=1.0,
width=0.2,
depth=0.05,
height=0.3):
parser = Parser(plant)
bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf"))
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("bin_base", bin),
RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
[0, 0, -0.015]))
planar_joint_frame = plant.AddFrame(
FixedOffsetFrame(
"planar_joint_frame", plant.world_frame(),
RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2))))
# TODO(russt): make this a *random* box?
# TODO(russt): move random box to a shared py file.
box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu)
box_joint = plant.AddJoint(
PlanarJoint("box", planar_joint_frame,
plant.GetFrameByName("box", box_instance)))
box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi])
box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2])
box_joint.set_default_translation([0, depth / 2.0])
return box_instance
def __init__(self, name, tf):
# Handle geometry and physics.
PhysicsGeometryNodeMixin.__init__(self,
tf=tf,
fixed=True,
is_container=True)
geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 0., 0., 0., 0.]))
# TODO(gizatt) Resource path management to be done here...
model_path = "/home/gizatt/drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf"
self.register_model_file(tf=geom_tf,
model_path=model_path,
root_body_name="link")
geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 0.6251, 0., 0., 0.]))
self.register_clearance_geometry(tf=geom_tf,
geometry=Box(width=1.,
depth=1.,
height=1.0))
# Put an object spawning volume on the table surface.
rules = []
rules.append(
DeterministicRelativePoseProductionRule(
child_constructor=PlanarObjectRegion,
child_name="table_object_region",
relative_tf=pose_to_tf_matrix(
torch.tensor([0., 0., 0.8, 0., 0., 0.])),
object_production_rate=0.5,
bounds=((-0.25, 0.25), (-0.25, 0.25), (0., 0.2))))
AndNode.__init__(self, name=name, production_rules=rules)
def __init__(self, name, tf):
# Handle geometry and physics.
PhysicsGeometryNodeMixin.__init__(self,
tf=tf,
fixed=True,
is_container=True)
# Offset cabinet geometry from the wall
geom_tf = pose_to_tf_matrix(torch.tensor([0.15, 0., 0., 0., 0., 0.]))
# TODO(gizatt) Resource path management to be done here...
model_path = "/home/gizatt/drake/examples/manipulation_station/models/cupboard.sdf"
# Randomly open doors random amounts.
# Left door is straight open at -pi/2 and closed at 0.
left_door_state = pyro.sample("%s_left_door_state" % name,
dist.Uniform(-np.pi / 2., 0.))
# Right door is straight open at pi/2 and closed at 0.
right_door_state = pyro.sample("%s_right_door_state" % name,
dist.Uniform(0.0, np.pi / 2.))
self.register_model_file(tf=geom_tf,
model_path=model_path,
root_body_name="cupboard_body",
q0_dict={
"left_door_hinge":
left_door_state.detach().numpy(),
"right_door_hinge":
right_door_state.detach().numpy()
})
# Add clearance geometry to indicate that shelves shouldn't
# penetrate each other and should have clearance to open the doors.
clearance_depth = 0.75
# Offset out from wall just a bit more to give collision detection
# a margin
geom_tf = pose_to_tf_matrix(
torch.tensor(
[clearance_depth / 2. + 0.001, 0., 0., 0., 0., np.pi / 2.]))
geometry = Box(width=0.6, depth=clearance_depth, height=1.)
self.register_clearance_geometry(geom_tf, geometry)
# Place shelf nodes.
# Dimensions of a single shelf, in terms of the
shelf_height = 0.13115 * 2
bottom_shelf_z_local = -0.3995
num_shelves = 3
rules = []
for k in range(num_shelves):
rules.append(
DeterministicRelativePoseProductionRule(
child_constructor=PlanarObjectRegion,
child_name="cabinet_level_%02d" % k,
relative_tf=pose_to_tf_matrix(
torch.tensor([
0.15, 0., bottom_shelf_z_local + shelf_height * k,
0., 0., 0.
])),
object_production_rate=0.5,
bounds=((-0.1, 0.1), (-0.2, 0.2), (0., 0.2))))
AndNode.__init__(self, name=name, production_rules=rules)
def __init__(self, name, tf):
PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
# Only has clearance geometry: indicates robot start location, and
# ensures that there's space to put a robot into this scene.
geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 1., 0., 0., 0.]))
geometry = Box(
width=1.,
depth=1.,
height=2.,
)
self.register_clearance_geometry(geom_tf, geometry)
TerminalNode.__init__(self, name=name)
def AddShape(plant, shape, name, mass=1, mu=1, color=[.5, .5, .9, 1.0]):
instance = plant.AddModelInstance(name)
# TODO: Add a method to UnitInertia that accepts a geometry shape (unless
# that dependency is somehow gross) and does this.
if isinstance(shape, Box):
inertia = UnitInertia.SolidBox(shape.width(), shape.depth(),
shape.height())
elif isinstance(shape, Cylinder):
inertia = UnitInertia.SolidCylinder(shape.radius(), shape.length())
elif isinstance(shape, Sphere):
inertia = UnitInertia.SolidSphere(shape.radius())
else:
raise RunTimeError(
f"need to write the unit inertia for shapes of type {shape}")
body = plant.AddRigidBody(
name, instance,
SpatialInertia(mass=mass,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=inertia))
if plant.geometry_source_is_registered():
if isinstance(shape, Box):
plant.RegisterCollisionGeometry(
body, RigidTransform(),
Box(shape.width() - 0.001,
shape.depth() - 0.001,
shape.height() - 0.001), name, CoulombFriction(mu, mu))
i = 0
for x in [-shape.width() / 2.0, shape.width() / 2.0]:
for y in [-shape.depth() / 2.0, shape.depth() / 2.0]:
for z in [-shape.height() / 2.0, shape.height() / 2.0]:
plant.RegisterCollisionGeometry(
body, RigidTransform([x, y, z]),
Sphere(radius=1e-7), f"contact_sphere{i}",
CoulombFriction(mu, mu))
i += 1
else:
plant.RegisterCollisionGeometry(body, RigidTransform(), shape, name,
CoulombFriction(mu, mu))
plant.RegisterVisualGeometry(body, RigidTransform(), shape, name, color)
return instance
def __init__(self,
name,
tf,
object_production_rate,
bounds,
show_geometry=False):
PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
self.x_bounds = bounds[0]
self.y_bounds = bounds[1]
self.z_bounds = bounds[2]
# Add some geometry for viz purposes
geom_tf = pose_to_tf_matrix(
torch.tensor([
np.mean(self.x_bounds),
np.mean(self.y_bounds),
np.mean(self.z_bounds), 0., 0., 0.
]))
geometry = Box(width=self.x_bounds[1] - self.x_bounds[0],
depth=self.y_bounds[1] - self.y_bounds[0],
height=self.z_bounds[1] - self.z_bounds[0])
if show_geometry:
self.register_visual_geometry(geom_tf,
geometry,
color=np.array([0.5, 1.0, 0.2, 0.2]))
style_group_options = ["utensils", "foodstuffs"]
# Do foodstuffs more often than plates and things
style_group_k = pyro.sample("%s_style" % name,
dist.Categorical(torch.tensor([0.3, 0.7
]))).item()
style_group = style_group_options[style_group_k]
# Produce a geometric number of objects within bounds.
object_production_rule = RandomRelativePoseProductionRule(
RandomKitchenStuff,
"%s_object" % name,
self._sample_object_pose,
style_group=style_group)
GeometricSetNode.__init__(self,
name=name,
production_rule=object_production_rule,
geometric_prob=object_production_rate)
def add_geometry(plant, body):
box = Box(
width=0.2,
depth=0.2,
height=0.2,
)
plant.RegisterVisualGeometry(
body=body,
X_BG=RigidTransform(),
shape=box,
name=f"visual",
diffuse_color=[1, 1, 1, 1],
)
static_friction = 1.
plant.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=box,
name="collision",
coulomb_friction=CoulombFriction(
static_friction=1,
dynamic_friction=1,
))
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,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
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))
# Figure out what YCB objects we have available to add.
ycb_object_dir = os.path.join(
getDrakePath(), "manipulation/models/ycb/sdf/")
ycb_object_sdfs = os.listdir(ycb_object_dir)