def solve_inverse_kinematics(mbp, target_frame, target_pose,
max_position_error=0.005, theta_bound=0.01*np.pi, initial_guess=None):
if initial_guess is None:
initial_guess = np.zeros(mbp.num_positions())
for joint in prune_fixed_joints(get_joints(mbp)):
lower, upper = get_joint_limits(joint)
if -np.inf < lower < upper < np.inf:
initial_guess[joint.position_start()] = random.uniform(lower, upper)
assert mbp.num_positions() == len(initial_guess)
ik_scene = InverseKinematics(mbp)
world_frame = mbp.world_frame()
ik_scene.AddOrientationConstraint(
frameAbar=target_frame, R_AbarA=RotationMatrix.Identity(),
frameBbar=world_frame, R_BbarB=RotationMatrix(target_pose.rotation()),
theta_bound=theta_bound)
lower = target_pose.translation() - max_position_error
upper = target_pose.translation() + max_position_error
ik_scene.AddPositionConstraint(
frameB=target_frame, p_BQ=np.zeros(3),
frameA=world_frame, p_AQ_lower=lower, p_AQ_upper=upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), initial_guess)
result = prog.Solve()
if result != SolutionResult.kSolutionFound:
return None
return prog.GetSolution(ik_scene.q())
def AddMeshcatTriad(meshcat,
path,
length=.25,
radius=0.01,
opacity=1.,
X_PT=RigidTransform()):
meshcat.SetTransform(path, X_PT)
# x-axis
X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2., 0, 0])
meshcat.SetTransform(path + "/x-axis", X_TG)
meshcat.SetObject(path + "/x-axis", Cylinder(radius, length),
Rgba(1, 0, 0, opacity))
# y-axis
X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2., 0])
meshcat.SetTransform(path + "/y-axis", X_TG)
meshcat.SetObject(path + "/y-axis", Cylinder(radius, length),
Rgba(0, 1, 0, opacity))
# z-axis
X_TG = RigidTransform([0, 0, length / 2.])
meshcat.SetTransform(path + "/z-axis", X_TG)
meshcat.SetObject(path + "/z-axis", Cylinder(radius, length),
Rgba(0, 0, 1, opacity))
def test_orientation_constraint(self, variables):
constraint = ik.OrientationConstraint(
plant=variables.plant,
frameAbar=variables.body1_frame, R_AbarA=RotationMatrix(),
frameBbar=variables.body2_frame, R_BbarB=RotationMatrix(),
theta_bound=0.2 * math.pi, plant_context=variables.plant_context)
self.assertIsInstance(constraint, mp.Constraint)
def GetHomeConfiguration(is_printing=True):
"""
Returns a configuration of the MultibodyPlant in which point Q (defined by global variable p_EQ)
in robot EE frame is at p_WQ_home, and orientation of frame Ea is R_WEa_ref.
"""
# get "home" pose
ik_scene = inverse_kinematics.InverseKinematics(plant)
theta_bound = 0.005 * np.pi # 0.9 degrees
X_EEa = GetEndEffectorWorldAlignedFrame()
R_EEa = RotationMatrix(X_EEa.rotation())
ik_scene.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WL7_ref,
frameBbar=l7_frame,
R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
p_WQ0 = p_WQ_home
p_WQ_lower = p_WQ0 - 0.005
p_WQ_upper = p_WQ0 + 0.005
ik_scene.AddPositionConstraint(frameB=l7_frame,
p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions()))
result = prog.Solve()
if is_printing:
print result
return prog.GetSolution(ik_scene.q())
def test_AddOrientationCost(self):
binding = self.ik_two_bodies.AddOrientationCost(
frameAbar=self.body1_frame,
R_AbarA=RotationMatrix(),
frameBbar=self.body2_frame,
R_BbarB=RotationMatrix(),
c=1.0)
self.assertIsInstance(binding, mp.Binding[mp.Cost])
def test_orientation_cost(self, variables):
cost = ik.OrientationCost(plant=variables.plant,
frameAbar=variables.body1_frame,
R_AbarA=RotationMatrix(),
frameBbar=variables.body2_frame,
R_BbarB=RotationMatrix(),
c=1.0,
plant_context=variables.plant_context)
self.assertIsInstance(cost, mp.Cost)
def add_triad(
source_id,
frame_id,
scene_graph,
*,
length,
radius,
opacity,
X_FT=RigidTransform(),
name="frame",
):
"""
Adds illustration geometry representing the coordinate frame, with the
x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes
point in +x, +y and +z directions, respectively.
Based on [code permalink](https://github.com/RussTedrake/manipulation/blob/5e59811/manipulation/scenarios.py#L367-L414).# noqa
Args:
source_id: The source registered with SceneGraph.
frame_id: A geometry::frame_id registered with scene_graph.
scene_graph: The SceneGraph with which we will register the geometry.
length: the length of each axis in meters.
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
X_FT: a RigidTransform from the triad frame T to the frame_id frame F
name: the added geometry will have names name + " x-axis", etc.
"""
# x-axis
X_TG = RigidTransform(
RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2.0, 0, 0],
)
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " x-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([1, 0, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# y-axis
X_TG = RigidTransform(
RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2.0, 0],
)
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " y-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 1, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# z-axis
X_TG = RigidTransform([0, 0, length / 2.0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " z-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 0, 1, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
def OutputGeometryPose(self, context, poses):
assert self.body_frame_id.is_valid()
assert self.elevator_frame_id.is_valid()
lh = 0.317 # elevator hinge.
state = GliderState(self.get_input_port(0).Eval(context))
body_pose = RigidTransform(RotationMatrix.MakeYRotation(state.pitch),
[state.x, 0, state.z])
elevator_pose = RigidTransform(
RotationMatrix.MakeYRotation(state.pitch + state.elevator), [
state.x - lh * np.cos(state.pitch), 0,
state.z + lh * np.sin(state.pitch)
])
poses.get_mutable_value().set_value(self.body_frame_id, body_pose)
poses.get_mutable_value().set_value(self.elevator_frame_id,
elevator_pose)
def AddTriad(source_id,
frame_id,
scene_graph,
length=.25,
radius=0.01,
opacity=1.,
X_FT=RigidTransform(),
name="frame"):
"""
Adds illustration geometry representing the coordinate frame, with the
x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes
point in +x, +y and +z directions, respectively.
Args:
source_id: The source registered with SceneGraph.
frame_id: A geometry::frame_id registered with scene_graph.
scene_graph: The SceneGraph with which we will register the geometry.
length: the length of each axis in meters.
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
X_FT: a RigidTransform from the triad frame T to the frame_id frame F
name: the added geometry will have names name + " x-axis", etc.
"""
# x-axis
X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2., 0, 0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " x-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([1, 0, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# y-axis
X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2., 0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " y-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 1, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# z-axis
X_TG = RigidTransform([0, 0, length / 2.])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " z-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 0, 1, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
def SolveOneShotIk(
p_WQ_ref,
R_WL7_ref,
p_L7Q,
q_initial_guess,
position_tolerance=0.005,
theta_bound=0.005):
ik = inverse_kinematics.InverseKinematics(plant)
ik.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7_ref,
frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
p_WQ_lower = p_WQ_ref - position_tolerance
p_WQ_upper = p_WQ_ref + position_tolerance
ik.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_lower, p_AQ_upper=p_WQ_upper)
prog = ik.prog()
prog.SetInitialGuess(ik.q(), q_initial_guess)
result = mp.Solve(prog)
if result.get_solution_result() != SolutionResult.kSolutionFound:
print(result.get_solution_result())
raise RuntimeError
return result.GetSolution(ik.q())
def xyz_rpy_deg(xyz, rpy_deg):
"""Shorthand to create a rigid transform from XYZ and RPY (degrees)."""
rpy = np.deg2rad(rpy_deg)
return RigidTransform(
R=RotationMatrix(rpy=RollPitchYaw(rpy), ),
p=xyz,
)
def _to_pose(position, quaternion):
"""Given pose parts of an lcmt_viewer_geometry_data, parse it into a
RigidTransform."""
(p_x, p_y, p_z) = position
(q_w, q_x, q_y, q_z) = quaternion
return RigidTransform(R=RotationMatrix(quaternion=Quaternion(
w=q_w,
x=q_x,
y=q_y,
z=q_z,
), ),
p=(p_x, p_y, p_z))
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_):
int_marker = InteractiveMarker()
int_marker.header.frame_id = revolute_joint.child_body().body_frame().name()
int_marker.name = revolute_joint.name()
int_marker.scale = 0.3
int_marker.pose.position.x = 0.
int_marker.pose.position.y = 0.
int_marker.pose.position.z = 0.
int_marker.pose.orientation.w = 1.
int_marker.pose.orientation.x = 0.
int_marker.pose.orientation.y = 0.
int_marker.pose.orientation.z = 0.
# Drake revolute axis is in frame F on parent
axis_hat = revolute_joint.revolute_axis()
self._joint_axis_in_child[revolute_joint.name()] = axis_hat
# What rotation would get the parent X axis to align with the joint axis?
rotation_matrix = ComputeBasisFromAxis(0, axis_hat)
pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion()
joint_control = InteractiveMarkerControl()
joint_control.orientation.w = pydrake_quat.w()
joint_control.orientation.x = pydrake_quat.x()
joint_control.orientation.y = pydrake_quat.y()
joint_control.orientation.z = pydrake_quat.z()
joint_control.always_visible = True
joint_control.name = f'rotate_axis_{revolute_joint.name()}'
joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(joint_control)
return int_marker
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
def get_q_object_final(self, q_object_init, ee_init, ee_final):
"""
Input:
q_object_init: [quaternion, xyz]
ee_init: [x y z r p y]
ee_final: [x y z r p y]
xyz_iiwa: [x, y, z]
Return:
q_object_final: [quaternion, xyz]
"""
xyz_obj = np.array(q_object_init[4:]).transpose()
# xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame
quaternion_obj = Quaternion(np.array(q_object_init[:4]))
rot_obj = RotationMatrix(quaternion_obj)
X_WO = RigidTransform(rot_obj, xyz_obj)
xyz_ee_init = np.array(ee_init[:3]).transpose()
rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix()
X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init)
xyz_ee_final = np.array(ee_final[:3]).transpose()
rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix()
X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final)
X_EO = X_WE1.inverse().multiply(X_WO)
X_WO2 = X_WE2.multiply(X_EO)
wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz()
xyz_obj_final = X_WO2.translation()
# xyz_ee_obj_init = xyz_obj - xyz_ee_init
# rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose())
# xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init)
# # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame
# xyz_obj_final = xyz_ee_final + xyz_ee_obj_final
# rot_obj_final = rot_ee_init_final.multiply(rot_obj)
# quaternion_obj_final = rot_obj_final.ToQuaternion()
# wxyz_obj_final = quaternion_obj_final.wxyz()
q_object_final = []
for i in range(4):
q_object_final.append(wxyz_obj_final[i])
for i in range(3):
q_object_final.append(xyz_obj_final[i])
return q_object_final
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(quaternion=Quaternion(1.0 / 3, 2.0 /
3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(frameAbar=self.body1_frame,
R_AbarA=R_AbarA,
frameBbar=self.body2_frame,
R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(self.prog.Solve(),
mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def get_box_from_geom(scene_graph, visual_only=True):
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
if visual_only and (geom.color[3] == 0):
continue
visual_index += 1
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height/2.])
# In Drake, cylinders are along +z
else:
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_):
int_marker = InteractiveMarker()
int_marker.header.frame_id = revolute_joint.child_body().body_frame().name()
int_marker.name = revolute_joint.name()
int_marker.scale = 0.3
int_marker.pose.position.x = 0.
int_marker.pose.position.y = 0.
int_marker.pose.position.z = 0.
int_marker.pose.orientation.w = 1.
int_marker.pose.orientation.x = 0.
int_marker.pose.orientation.y = 0.
int_marker.pose.orientation.z = 0.
# Drake revolute axis is in frame F on parent
axis_hat = revolute_joint.revolute_axis()
self._joint_axis_in_child[revolute_joint.name()] = axis_hat
# What rotation would get the parent X axis to align with the joint axis?
# https://math.stackexchange.com/q/476311
x_axis = (1, 0, 0)
v = numpy.cross(x_axis, axis_hat)
c = numpy.dot(x_axis, axis_hat)
v_sub_x = numpy.array((
(0, -v[2], v[1]),
(v[2], 0, -v[0]),
(-v[1], v[0], 0)), dtype=numpy.float64)
v_sub_x_squared = numpy.dot(v_sub_x, v_sub_x)
rotation_matrix = numpy.eye(3) + v_sub_x + v_sub_x_squared * (1.0 / (1.0 + c))
pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion()
joint_control = InteractiveMarkerControl()
joint_control.orientation.w = pydrake_quat.w()
joint_control.orientation.x = pydrake_quat.x()
joint_control.orientation.y = pydrake_quat.y()
joint_control.orientation.z = pydrake_quat.z()
joint_control.always_visible = True
joint_control.name = f'rotate_axis_{revolute_joint.name()}'
joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(joint_control)
return int_marker
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def _convert_mesh(geom):
# Given a LCM geometry message, forms a meshcat geometry and material
# for that geometry, as well as a local tf to the meshcat geometry
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# both fields are returned as None.
meshcat_geom = None
material = None
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = (
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3]))
elif geom.type == geom.MESH:
meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png, since they're
# both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material information
# should be gleaned from the SceneGraph constituents themselves, so
# that we visualize what the simulation is *actually* reasoning about
# rather than what files happen to be present.
candidate_texture_path_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const",
type=float,
default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def setUp(self):
self.points = np.array([[1, 1, 0], [2, 1, 0]]).T
self.translation = RigidTransform(p=[1, 2, 3])
self.rotation = RigidTransform(
RotationMatrix(RollPitchYaw(0, 0, np.pi / 2)))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
'--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
"-w",
"--open-window",
dest="browser_new",
action="store_const",
const=1,
default=None,
help="Open the MeshCat display in a new browser window.")
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (joint sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a
# config file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
geometry_query_port = station.GetOutputPort("geometry_query")
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
if args.setup == 'planar':
meshcat.Set2dRenderMode()
pyplot_visualizer = ConnectPlanarSceneGraphVisualizer(
builder, station.get_scene_graph(), geometry_query_port)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
teleop = builder.AddSystem(
JointSliders(meshcat=meshcat, plant=station.get_controller_plant()))
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
if args.test:
iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
teleop.SetPositions(q0)
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def load(self, context=None):
"""
Loads ``meshcat`` visualization elements.
Precondition:
Either the context is a valid Context for this system with the
geometry_query port connected or the ``scene_graph`` passed in the
constructor must be a valid SceneGraph.
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
if context and self.get_geometry_query_input_port().HasValue(context):
inspector = self.get_geometry_query_input_port().Eval(
context).inspector()
elif self._scene_graph:
inspector = self._scene_graph.model_inspector()
else:
raise RuntimeError(
"You must provide a valid Context for this system with the "
"geometry_query port connected or the ``scene_graph`` passed "
"in the constructor must be a valid SceneGraph.")
vis = self.vis[self.prefix]
# Make a fixed-seed generator for random colors for bodies.
color_generator = np.random.RandomState(seed=42)
for frame_id in inspector.GetAllFrameIds():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, self._role)
if count == 0:
continue
if frame_id == inspector.world_frame_id():
name = "world"
else:
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = (inspector.GetOwningSourceName(frame_id) + "/" +
inspector.GetName(frame_id).replace("::", "/"))
frame_vis = vis[name]
for g_id in inspector.GetGeometries(frame_id, self._role):
color = 0xe5e5e5 # default color
alpha = 1.0
hydro_mesh = None
if self._role == Role.kIllustration:
props = inspector.GetIllustrationProperties(g_id)
if props and props.HasProperty("phong", "diffuse"):
rgba = props.GetProperty("phong", "diffuse")
# Convert Rgba from [0-1] to hex 0xRRGGBB.
color = int(255 * rgba.r()) * 256**2
color += int(255 * rgba.g()) * 256
color += int(255 * rgba.b())
alpha = rgba.a()
elif self._role == Role.kProximity:
# Pick a random color to make collision geometry
# visually distinguishable.
color = color_generator.randint(2**(24))
if self._prefer_hydro:
hydro_mesh = inspector. \
maybe_get_hydroelastic_mesh(g_id)
material = g.MeshLambertMaterial(color=color,
transparent=alpha != 1.,
opacity=alpha)
shape = inspector.GetShape(g_id)
X_FG = inspector.GetPoseInFrame(g_id).GetAsMatrix4()
if hydro_mesh is not None:
# We've got a hydroelastic mesh to load.
surface_mesh = hydro_mesh
if isinstance(hydro_mesh, VolumeMesh):
surface_mesh = ConvertVolumeToSurfaceMesh(hydro_mesh)
v_count = len(surface_mesh.triangles()) * 3
vertices = np.empty((v_count, 3), dtype=float)
normals = np.empty((v_count, 3), dtype=float)
mesh_verts = surface_mesh.vertices()
v = 0
for face in surface_mesh.triangles():
p_MA = mesh_verts[int(face.vertex(0))]
p_MB = mesh_verts[int(face.vertex(1))]
p_MC = mesh_verts[int(face.vertex(2))]
vertices[v, :] = tuple(p_MA)
vertices[v + 1, :] = tuple(p_MB)
vertices[v + 2, :] = tuple(p_MC)
p_AB_M = p_MB - p_MA
p_AC_M = p_MC - p_MA
n_M = np.cross(p_AB_M, p_AC_M)
nhat_M = n_M / np.sqrt(n_M.dot(n_M))
normals[v, :] = nhat_M
normals[v + 1, :] = nhat_M
normals[v + 2, :] = nhat_M
v += 3
geom = HydroTriSurface(vertices, normals)
elif isinstance(shape, Box):
geom = g.Box(
[shape.width(),
shape.depth(),
shape.height()])
elif isinstance(shape, Sphere):
geom = g.Sphere(shape.radius())
elif isinstance(shape, Cylinder):
geom = g.Cylinder(shape.length(), shape.radius())
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
R_GC = RotationMatrix.MakeXRotation(np.pi / 2.0).matrix()
X_FG[0:3, 0:3] = X_FG[0:3, 0:3].dot(R_GC)
elif isinstance(shape, (Mesh, Convex)):
geom = g.ObjMeshGeometry.from_file(shape.filename()[0:-3] +
"obj")
# Attempt to find a texture for the object by looking for
# an identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png,
# since they're both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material
# information should be gleaned from the SceneGraph
# constituents themselves, so that we visualize what the
# simulation is *actually* reasoning about rather than what
# files happen to be present.
candidate_texture_path = shape.filename()[0:-3] + "png"
if os.path.exists(candidate_texture_path):
material = g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(
candidate_texture_path)))
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "mesh"))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "material"))
X_FG = X_FG.dot(tf.scale_matrix(shape.scale()))
else:
warnings.warn(f"Unsupported shape {shape} ignored")
continue
geometry_vis = frame_vis[str(g_id.get_value())]
geometry_vis.set_object(geom, material)
geometry_vis.set_transform(X_FG)
if frame_id in self.frames_to_draw:
AddTriad(self.vis,
name=name,
prefix=self.prefix + "/" + name,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
self.frames_to_draw.remove(frame_id)
if frame_id != inspector.world_frame_id():
self._dynamic_frames.append({
"id": frame_id,
"name": name,
})
# Loop through the input frames_to_draw list and warn the user if the
# frame_id does not exist in the scene graph.
for frame_id in self.frames_to_draw:
warnings.warn(f"Non-existent frame {frame_id} ignored")
continue
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
# Test getters.
pq = PiecewiseQuaternionSlerp(
breaks=[0, 1], angle_axes=[a, AngleAxis(np.pi / 2, [0, 0, 1])])
np.testing.assert_equal(np.array([1, 0, 0, 0]),
pq.orientation(time=0).wxyz())
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=0),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=0))
np.testing.assert_allclose(
np.array([np.cos(np.pi / 4), 0, 0,
np.sin(np.pi / 4)]),
pq.orientation(time=1).wxyz(),
atol=1e-15,
rtol=0,
)
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=1),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=1))
def _build_body_patches(self, use_random_colors,
substitute_collocated_mesh_files):
"""
Generates body patches. self._patch_Blist stores a list of patches for
each body (starting at body id 1). A body patch is a list of all 3D
vertices of a piece of visual geometry.
"""
self._patch_Blist = {}
self._patch_Blist_colors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
# TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking
# LCM and inspecting the generated message.
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick random colors
# for some bodies. Each body will be given a unique color when using
# this random generator, with each visual element of the body colored
# the same.
color = iter(
plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
X_BG = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch_G = np.vstack(
(geom.float_data[0] / 2. *
np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
geom.float_data[1] / 2. *
np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
geom.float_data[2] / 2. *
np.array([-1, -1, -1, -1, 1, 1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch_G = np.vstack([
np.sin(lati) * np.cos(longi),
np.sin(lati) * np.sin(longi),
np.cos(lati)
])
patch_G *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch_G = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
filename = geom.string_data
base, ext = os.path.splitext(filename)
if (ext.lower() is not ".obj") and \
substitute_collocated_mesh_files:
# Check for a co-located .obj file (case insensitive).
for f in glob.glob(base + '.*'):
if f[-4:].lower() == '.obj':
filename = f
break
if filename[-4:].lower() != '.obj':
raise RuntimeError("The given file " + filename +
" is not "
"supported and no alternate " +
base + ".obj could be found.")
if not os.path.exists(filename):
raise FileNotFoundError(errno.ENOENT,
os.strerror(errno.ENOENT),
filename)
mesh = ReadObjToSurfaceMesh(filename)
patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Compute pose in body.
patch_B = X_BG @ patch_G
# Close path if not closed.
if (patch_B[:, -1] != patch_B[:, 0]).any():
patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T))
this_body_patches.append(patch_B)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self._patch_Blist[link.name] = this_body_patches
self._patch_Blist_colors[link.name] = this_body_colors
def buildViewPatches(self, use_random_colors):
''' Generates view patches. self.viewPatches stores a list of
viewPatches for each body (starting at body id 1). A viewPatch is a
list of all 3D vertices of a piece of visual geometry. '''
self.viewPatches = {}
self.viewPatchColors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick
# random colors for some bodies. Each body will be given
# a unique color when using this random generator, with
# each visual element of the body colored the same.
color = iter(
plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch = np.vstack((geom.float_data[0] / 2. *
np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
geom.float_data[1] / 2. *
np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
geom.float_data[2] / 2. *
np.array([-1, -1, -1, -1, 1, 1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch = np.vstack([
np.sin(longi) * np.cos(lati),
np.sin(longi) * np.sin(lati),
np.cos(lati)
])
patch *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
# TODO(gizatt): Remove trimesh and shapely dependency when
# vertex information is accessible from the SceneGraph
# interface.
mesh = trimesh.load(geom.string_data)
patch = mesh.vertices.T
# Apply scaling
for i in range(3):
patch[i, :] *= geom.float_data[i]
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
patch = np.vstack((patch, np.ones((1, patch.shape[1]))))
patch = np.dot(element_local_tf.GetAsMatrix4(), patch)
# Close path if not closed
if (patch[:, -1] != patch[:, 0]).any():
patch = np.hstack((patch, patch[:, 0][np.newaxis].T))
this_body_patches.append(patch)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self.viewPatches[link.name] = this_body_patches
self.viewPatchColors[link.name] = this_body_colors
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const", type=float, default=0.1,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
# If using meshcat, don't render the cameras, since RgbdCamera
# rendering only works with drake-visualizer. Without this check,
# running this code in a docker container produces libGL errors.
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
elif args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
else:
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
image_to_lcm_image_array = builder.AddSystem(
ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
for name in station.get_camera_names():
cam_port = (
image_to_lcm_image_array
.DeclareImageInputPort[PixelType.kRgba8U](
"camera_" + name))
builder.Connect(
station.GetOutputPort("camera_" + name + "_rgb_image"),
cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=image_array_t,
lcm=None,
publish_period=0.1,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(
image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
if args.setup == 'planar':
# Extract the 3 joints that are not welded in the planar version.
iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2]
# The below constant is in body frame.
params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(EndEffectorTeleop(args.setup == 'planar'))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
num_iiwa_joints = station.num_iiwa_joints()
if args.test:
iiwa_velocities = builder.AddSystem(SignalLogger(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--time_step", type=float, default=0.005,
help="Time constant for the differential IK solver and first order"
"low pass filter applied to the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
params.set_timestep(args.time_step)
# True velocity limits for the IIWA14 (in rad/s, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, args.time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick()))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.time_step, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort(
"wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter_, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# The contact frame's origin is located at contact_point and is
# oriented such that Cy is aligned with the contact force.
p_WC = contact_info.contact_point() # documented as in world.
if force_norm < 1e-6:
# We cannot rely on self._force_threshold to determine if the
# force can be normalized; that threshold can be zero.
R_WC = RotationMatrix()
else:
fhat_C_W = contact_info.contact_force() / force_norm
R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W,
axis_index=1)
X_WC = RigidTransform(R=R_WC, p=p_WC)
cvis.set_transform(X_WC.GetAsMatrix4())
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
