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