def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEquals(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance( value.get_isometry(), Isometry3)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - - Value. self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected))
def test_roll_pitch_yaw(self): rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.get_roll_angle(), rpy.get_pitch_angle(), rpy.get_yaw_angle()), (0, 0, 0)) q_I = Quaternion() self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz()))
def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi self.ik_two_bodies.AddOrientationConstraint( frameA=self.body1_frame, frameB=self.body2_frame, 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_AB = body1_rotmat.transpose().dot(body2_rotmat) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle.set_pose(kIndex, Isometry3(q, p)) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) self.assertTrue( (bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3)))
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3)))
def test_rotation_matrix(self): R = mut.RotationMatrix() self.assertTrue(np.allclose(R.matrix(), np.eye(3))) self.assertTrue( np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3))) R = mut.RotationMatrix(quaternion=Quaternion.Identity()) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0])) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) # - Nontrivial quaternion. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) R = mut.RotationMatrix(quaternion=q) q_R = R.ToQuaternion() self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz())) # - Inverse. R_I = R.inverse().multiply(R) self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
def test_rotation_matrix(self): R = mut.RotationMatrix() self.assertTrue(np.allclose(R.matrix(), np.eye(3))) self.assertTrue(np.allclose( mut.RotationMatrix.Identity().matrix(), np.eye(3))) R = mut.RotationMatrix(quaternion=Quaternion.Identity()) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0])) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) # - Nontrivial quaternion. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) R = mut.RotationMatrix(quaternion=q) q_R = R.ToQuaternion() self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz())) # - Inverse. R_I = R.inverse().multiply(R) self.assertTrue(np.allclose(R_I.matrix(), np.eye(3)))
def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_AddPositionConstraint(self): p_BQ = np.array([0.2, 0.3, 0.5]) p_AQ_lower = np.array([-0.1, -0.2, -0.3]) p_AQ_upper = np.array([-0.05, -0.12, -0.28]) self.ik_two_bodies.AddPositionConstraint( frameB=self.body1_frame, p_BQ=p_BQ, frameA=self.body2_frame, p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_AQ = body2_rotmat.transpose().dot( body1_rotmat.dot(p_BQ) + body1_pos - body2_pos) self.assertTrue(np.greater(p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all()) self.assertTrue(np.less(p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all())
def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) 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() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6)
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def test_rigid_transform(self): def check_equality(X_actual, X_expected_matrix): # TODO(eric.cousineau): Use `IsNearlyEqualTo`. self.assertIsInstance(X_actual, mut.RigidTransform) self.assertTrue( np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix)) # - Constructors. X_I = np.eye(4) check_equality(mut.RigidTransform(), X_I) check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I) R_I = mut.RotationMatrix() p_I = np.zeros(3) rpy_I = mut.RollPitchYaw(0, 0, 0) quaternion_I = Quaternion.Identity() angle = np.pi * 0 axis = [0, 0, 1] angle_axis = AngleAxis(angle=angle, axis=axis) check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I) check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I) check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I) check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I) check_equality(mut.RigidTransform(R=R_I), X_I) check_equality(mut.RigidTransform(p=p_I), X_I) # - Accessors, mutators, and general methods. X = mut.RigidTransform() X.set(R=R_I, p=p_I) X.SetFromIsometry3(pose=Isometry3.Identity()) check_equality(mut.RigidTransform.Identity(), X_I) self.assertIsInstance(X.rotation(), mut.RotationMatrix) X.set_rotation(R=R_I) self.assertIsInstance(X.translation(), np.ndarray) X.set_translation(p=np.zeros(3)) self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I)) self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3])) self.assertIsInstance(X.GetAsIsometry3(), Isometry3) check_equality(X.inverse(), X_I) self.assertIsInstance(X.multiply(other=mut.RigidTransform()), mut.RigidTransform) self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray)
def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = pydrake.math.RotationMatrix( quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = pydrake.math.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 test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_pose(0, Isometry3(q3, p3)) bundle_value = AbstractValue.Make(bundle) context.FixInputPort(0, pose1) context.FixInputPort(1, pose2) context.FixInputPort(2, velocity) context.FixInputPort(3, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) isom1_actual = Isometry3() isom1_actual.set_translation(p1) self.assertTrue( (value.get_pose(0).matrix() == isom1_actual.matrix()).all()) isom2_actual = Isometry3() isom2_actual.set_translation(p2) self.assertTrue( (value.get_pose(1).matrix() == isom2_actual.matrix()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.vis[self.prefix].delete() # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currenly 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).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") else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.type, " IGNORED" continue # 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. def Rgb2Hex(rgb): val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry .MeshLambertMaterial( color=Rgb2Hex(geom.color))) self.vis[self.prefix][source_name][str( link.robot_num)][frame_name][str(j)].set_transform( element_local_tf)
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ # TODO(russt): Declare an initialization event to publish this # pending resolution of #9842. # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] 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") else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.type, " IGNORED" continue self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry .MeshLambertMaterial( color=Rgb2Hex(geom.color))) self.vis[self.prefix][source_name][str( link.robot_num)][frame_name][str(j)].set_transform( element_local_tf)