def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
def get_label_db(
plant, # pydrake MultiBodyPlant
):
"""
Builds database that associates bodies and labels
:return: TinyDB database
:rtype:
"""
db = TinyDB(storage=MemoryStorage)
for i in range(plant.num_bodies()):
body = plant.get_body(BodyIndex(i))
model_instance_id = int(body.model_instance())
body_name = body.name()
model_name = plant.GetModelInstanceName(body.model_instance())
entry = {
'label': i,
'model_instance_id': model_instance_id,
'body_name': body_name,
'model_name': model_name
}
db.insert(entry)
return db
def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
self.assertIsInstance(contact_info.point_pair(),
PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(contact_results))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
def weld_paper_edge(self, pedestal_y_dim, pedestal_z_dim):
"""
Fixes an edge of the paper to the pedestal
"""
# Fix paper to object
self.plant.WeldFrames(
self.plant.world_frame(),
self.plant.get_body(BodyIndex(self.link_idxs[0])).body_frame(),
RigidTransform(RotationMatrix(
), [0, 0, pedestal_z_dim+self.z_dim/2])
)
def test_contact(self, T):
# PenetrationAsPointPair has been bound for non-symbolic types only.
PenetrationAsPointPair = PenetrationAsPointPair_[T]
PointPairContactInfo = PointPairContactInfo_[T]
ContactResults = ContactResults_[T]
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3,))
self.assertTrue(point_pair.p_WCb.shape == (3,))
self.assertTrue(isinstance(point_pair.depth, T))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(
bodyA_index=id_A, bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]), p_WC=np.array([0, 0, 0]),
separation_speed=0, slip_speed=0, point_pair=point_pair)
self.assertTrue(
isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(
isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3,))
self.assertTrue(contact_info.contact_point().shape == (3,))
self.assertTrue(isinstance(contact_info.slip_speed(), T))
self.assertIsInstance(
contact_info.point_pair(), PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.point_pair_contact_info(0),
PointPairContactInfo))
def _do_forward_kinematics(self, context, data):
joint_positions = self._joint_positions_port.Eval(context)
# Set the latest positions in the plant context
self._plant.SetPositions(self._plant_context, joint_positions)
world_frame = self._plant.world_frame()
transforms = []
for i in range(self._plant.num_bodies()):
body = self._plant.get_body(BodyIndex(i))
# calculate pose of body in world frame
body_frame = body.body_frame()
transforms.append(
self._plant.CalcRelativeTransform(self._plant_context,
world_frame, body_frame))
data.set_value(transforms)
def DoPublish(self, context, event):
# TODO(russt): Copied from meshcat_visualizer.py.
# Change this to declare a periodic event with a
# callback instead of overriding DoPublish, pending #9992.
LeafSystem.DoPublish(self, context, event)
self.callback_lock.acquire()
if self.stop:
self.stop = False
if context.get_time() > 0.5:
self.callback_lock.release()
raise StopIteration
#query_object = self.EvalAbstractInput(context, 0).get_value()
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
if self.grab_needs_update:
hydra_tf = self.grab_update_hydra_pose
self.grab_needs_update = False
# If grab point is colliding...
#print [x.distance for x in query_object.ComputeSignedDistanceToPoint(hydra_tf.matrix()[:3, 3])]
# Find closest body to current pose
grab_center = hydra_tf.matrix()[:3, 3]
closest_distance = np.inf
closest_body = self.mbp.get_body(BodyIndex(2))
for body_id in self.all_manipulable_body_ids:
body = self.mbp.get_body(body_id)
offset = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
dist = np.linalg.norm(grab_center - offset.translation())
if dist < closest_distance:
closest_distance = dist
closest_body = body
self.selected_body = closest_body
self.selected_body_init_offset = self.mbp.EvalBodyPoseInWorld(
self.mbp_context, self.selected_body)
self.callback_lock.release()
def get_label_db(self):
"""
Builds database that associates bodies and labels
:return: TinyDB database
"""
db = TinyDB(storage=MemoryStorage)
for i in range(self._mbp.num_bodies()):
body = self._mbp.get_body(BodyIndex(i))
model_instance_id = int(body.model_instance())
body_name = body.name()
model_name = self._mbp.GetModelInstanceName(body.model_instance())
entry = {
'label': i,
'model_instance_id': model_instance_id,
'body_name': body_name,
'model_name': model_name
}
db.insert(entry)
return db
def test_type_safe_indices(self):
self.assertEqual(world_index(), BodyIndex(0))
def test_multibody_plant_api_via_parsing(self):
# TODO(eric.cousineau): Decouple this when construction can be done
# without parsing.
# This a subset of `multibody_plant_sdf_parser_test.cc`.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(file_name)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant.Finalize()
benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
self.assertEqual(plant.num_bodies(), benchmark.num_bodies())
self.assertEqual(plant.num_joints(), benchmark.num_joints())
self.assertEqual(plant.num_actuators(), benchmark.num_actuators())
self.assertEqual(plant.num_model_instances(),
benchmark.num_model_instances() + 1)
self.assertEqual(plant.num_positions(), benchmark.num_positions())
self.assertEqual(plant.num_positions(model_instance=model_instance),
benchmark.num_positions())
self.assertEqual(plant.num_velocities(), benchmark.num_velocities())
self.assertEqual(plant.num_multibody_states(),
benchmark.num_multibody_states())
self.assertEqual(plant.num_actuated_dofs(),
benchmark.num_actuated_dofs())
self.assertTrue(plant.is_finalized())
self.assertTrue(plant.HasBodyNamed(name="Link1"))
self.assertTrue(
plant.HasBodyNamed(name="Link1", model_instance=model_instance))
self.assertTrue(plant.HasJointNamed(name="ShoulderJoint"))
self.assertTrue(
plant.HasJointNamed(name="ShoulderJoint",
model_instance=model_instance))
shoulder = plant.GetJointByName(name="ShoulderJoint")
self._test_joint_api(shoulder)
np.testing.assert_array_equal(shoulder.position_lower_limits(),
[-np.inf])
np.testing.assert_array_equal(shoulder.position_upper_limits(),
[np.inf])
self.assertIs(
shoulder,
plant.GetJointByName(name="ShoulderJoint",
model_instance=model_instance))
self._test_joint_actuator_api(
plant.GetJointActuatorByName(name="ElbowJoint"))
self._test_body_api(plant.GetBodyByName(name="Link1"))
self.assertIs(
plant.GetBodyByName(name="Link1"),
plant.GetBodyByName(name="Link1", model_instance=model_instance))
self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2)
self._test_frame_api(plant.GetFrameByName(name="Link1"))
self.assertIs(
plant.GetFrameByName(name="Link1"),
plant.GetFrameByName(name="Link1", model_instance=model_instance))
self.assertEqual(model_instance,
plant.GetModelInstanceByName(name="acrobot"))
self.assertIsInstance(plant.get_actuation_input_port(), InputPort)
self.assertIsInstance(plant.get_state_output_port(), OutputPort)
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=2):
plant.get_continuous_state_output_port()
plant.get_continuous_state_output_port(model_instance)
self.assertIsInstance(plant.get_contact_results_output_port(),
OutputPort)
self.assertIsInstance(plant.num_frames(), int)
self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
self.assertIsInstance(
plant.get_joint_actuator(actuator_index=JointActuatorIndex(0)),
JointActuator)
self.assertIsInstance(plant.get_frame(frame_index=FrameIndex(0)),
Frame)
self.assertEqual(
"acrobot",
plant.GetModelInstanceName(model_instance=model_instance))
}),
],
global_transform=Isometry3(
translation=[0, 0, 0],
quaternion=Quaternion(offset_quat_base)),
out_prefix="/tmp/ycb_scene_%03d/" % scene_k))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
blender_cam.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
for obj_id in obj_ids:
mbp.SetFreeBodyPose(
mbp_context,
body=mbp.get_body(BodyIndex(obj_id)),
X_WB=Isometry3(translation=[
np.random.randn() * 0.1,
np.random.randn() * 0.1,
np.random.uniform(0.1, 0.3)
],
quaternion=RollPitchYaw(
np.random.uniform(0, 2 * np.pi,
size=3)).ToQuaternion()))
# Project to nonpenetration to seed sim
q0 = mbp.GetPositions(mbp_context).copy()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
def __init__(self, plant, scene_graph, default_joint_angle=-np.pi/60,
damping=1e-5, stiffness=1e-3):
# Drake objects
self.plant = plant
self.scene_graph = scene_graph
# Geometric and physical quantities
# PROGRAMMING: Refactor constants.FRICTION to be constants.mu
self.mu = constants.FRICTION
self.default_joint_angle = default_joint_angle
self.link_width = PLYWOOD_LENGTH/2
self.y_dim = self.link_width * config.num_links.value
self.link_mass = self.x_dim*self.y_dim*self.z_dim*self.density
# Lists of internal Drake objects
self.link_idxs = []
self.joints = []
self.damping = damping
self.stiffness = stiffness
self.instance = self.plant.AddModelInstance(self.name)
for link_num in range(config.num_links.value):
# Initialize bodies and instances
paper_body = self.plant.AddRigidBody(
self.name + "_body" + str(link_num),
self.instance,
SpatialInertia(mass=self.link_mass,
# CoM at origin of body frame
p_PScm_E=np.array([0., 0., 0.]),
# Default moment of inertia for a solid box
G_SP_E=UnitInertia.SolidBox(
self.x_dim, self.link_width, self.z_dim))
)
if self.plant.geometry_source_is_registered():
# Set a box with link dimensions for collision geometry
self.plant.RegisterCollisionGeometry(
paper_body,
RigidTransform(), # Pose in body frame
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim), # Actual shape
self.name + "_body" + str(link_num), pydrake.multibody.plant.CoulombFriction(
self.mu, self.mu) # Friction parameters
)
# Set Set a box with link dimensions for visual geometry
self.plant.RegisterVisualGeometry(
paper_body,
RigidTransform(),
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim),
self.name + "_body" + str(link_num),
[0, 1, 0, 1]) # RGBA color
# Operations between adjacent links
if link_num > 0:
# Get bodies
paper1_body = self.plant.get_body(
BodyIndex(self.link_idxs[-1]))
paper2_body = self.plant.GetBodyByName(
self.name + "_body" + str(link_num), self.instance)
# Set up joints
paper1_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper1_body,
RigidTransform(RotationMatrix(),
[
0,
(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper1_hinge_frame)
paper2_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper2_body,
RigidTransform(RotationMatrix(),
[
0,
-(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper2_hinge_frame)
joint = self.plant.AddJoint(pydrake.multibody.tree.RevoluteJoint(
"paper_hinge_" + str(link_num),
paper1_hinge_frame,
paper2_hinge_frame,
[1, 0, 0],
damping=damping))
if isinstance(default_joint_angle, list):
joint.set_default_angle(
self.default_joint_angle[link_num])
else:
joint.set_default_angle(self.default_joint_angle)
self.plant.AddForceElement(
RevoluteSpring(joint, 0, self.stiffness))
self.joints.append(joint)
# Ignore collisions between adjacent links
geometries = self.plant.CollectRegisteredGeometries(
[paper1_body, paper2_body])
self.scene_graph.collision_filter_manager().Apply(
CollisionFilterDeclaration()
.ExcludeWithin(geometries))
self.link_idxs.append(int(paper_body.index()))