def test_abstract_output_port_eval(self): model_value = AbstractValue.Make("Hello World") source = ConstantValueSource(copy.copy(model_value)) context = source.CreateDefaultContext() output_port = source.get_output_port(0) value = output_port.Eval(context) self.assertEqual(type(value), type(model_value.get_value())) self.assertEqual(value, model_value.get_value()) value_abs = output_port.EvalAbstract(context) self.assertEqual(type(value_abs), type(model_value)) self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_publisher(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm) model = self._model_message() dut.set_publish_period(period=0.1) context = dut.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(model)) dut.Publish(context) raw = lcm.get_last_published_message("TEST_CHANNEL") value = quaternion_t.decode(raw) self.assert_lcm_equal(value, model)
def setUp(self): builder = DiagramBuilder() X_WP_0 = RigidTransform.Identity() X_WP_1 = RigidTransform.Identity() X_WP_1.set_translation([1.0, 0, 0]) id_list = ["0", "1"] self.pc_concat = builder.AddSystem(PointCloudConcatenation(id_list)) self.num_points = 10000 xyzs = np.random.uniform(-0.1, 0.1, (3, self.num_points)) # Only go to 254 to distinguish between point clouds with and without # color. rgbs = np.random.uniform(0., 254.0, (3, self.num_points)) self.pc = PointCloud(self.num_points, Fields(BaseField.kXYZs | BaseField.kRGBs)) self.pc.mutable_xyzs()[:] = xyzs self.pc.mutable_rgbs()[:] = rgbs self.pc_no_rgbs = PointCloud(self.num_points, Fields(BaseField.kXYZs)) self.pc_no_rgbs.mutable_xyzs()[:] = xyzs diagram = builder.Build() simulator = Simulator(diagram) self.context = diagram.GetMutableSubsystemContext( self.pc_concat, simulator.get_mutable_context()) self.context.FixInputPort( self.pc_concat.GetInputPort("X_FCi_0").get_index(), AbstractValue.Make(X_WP_0)) self.context.FixInputPort( self.pc_concat.GetInputPort("X_FCi_1").get_index(), AbstractValue.Make(X_WP_1))
def __init__(self, config_file, segment_scene_function=None, get_pose_function=None): """ A system that takes in 3 Drake PointClouds and ImageRgba8U from RGBDCameras and determines the pose of an object in them. The user must supply a segmentation function and pose alignment to determine the pose. If these functions aren't supplied, the returned pose will always be the 4x4 identity matrix. @param config_file str. A path to a .yml configuration file for the cameras. @param segment_scene_function A Python function that returns a subset of the scene point cloud. See self.SegmentScene for more details. @param get_pose_function A Python function that calculates a pose from a segmented point cloud. See self.GetPose for more details. @system{ @input_port{camera_left_rgb}, @input_port{camera_middle_rgb}, @input_port{camera_right_rgb}, @input_port{left_point_cloud}, @input_port{middle_point_cloud}, @input_port{right_point_cloud}, @output_port{X_WObject} } """ LeafSystem.__init__(self) # TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB # fields. self.left_rgb = self._DeclareAbstractInputPort( "camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.middle_rgb = self._DeclareAbstractInputPort( "camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.right_rgb = self._DeclareAbstractInputPort( "camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.left_depth = self._DeclareAbstractInputPort( "left_point_cloud", AbstractValue.Make(mut.PointCloud())) self.middle_depth = self._DeclareAbstractInputPort( "middle_point_cloud", AbstractValue.Make(mut.PointCloud())) self.right_depth = self._DeclareAbstractInputPort( "right_point_cloud", AbstractValue.Make(mut.PointCloud())) self._DeclareAbstractOutputPort("X_WObject", lambda: AbstractValue.Make( Isometry3.Identity()), self._DoCalcOutput) self.segment_scene_function = segment_scene_function self.get_pose_function = get_pose_function self._LoadConfigFile(config_file)
def _construct(self, nv, target_body_index, converter=None): LeafSystem_[T].__init__(self, converter=converter) self.set_name("applied_force_test_system") self.nv = nv self.target_body_index = target_body_index self.DeclareAbstractOutputPort( "spatial_forces_vector", lambda: AbstractValue.Make( VectorExternallyAppliedSpatialForced_[T]()), self.DoCalcAbstractOutput) self.DeclareVectorOutputPort( "generalized_forces", BasicVector_[T](self.nv), self.DoCalcVectorOutput)
def test_contact_results_to_lcm(self): # ContactResultsToLcmSystem file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant_[float]() Parser(plant).AddModelFromFile(file_name) plant.Finalize() contact_results_to_lcm = ContactResultsToLcmSystem(plant) context = contact_results_to_lcm.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(ContactResults_[float]())) output = contact_results_to_lcm.AllocateOutput() contact_results_to_lcm.CalcOutput(context, output) result = output.get_data(0) self.assertIsInstance(result, AbstractValue)
def test_context_api(self): # Capture miscellaneous functions not yet tested. model_value = AbstractValue.Make("Hello") class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self._DeclareContinuousState(1) self._DeclareDiscreteState(2) self._DeclareAbstractState(model_value.Clone()) system = TrivialSystem() context = system.CreateDefaultContext() self.assertTrue( context.get_state() is context.get_mutable_state()) self.assertTrue( context.get_continuous_state_vector() is context.get_mutable_continuous_state_vector()) self.assertTrue( context.get_discrete_state_vector() is context.get_mutable_discrete_state_vector()) self.assertEquals(context.get_num_abstract_states(), 1) self.assertTrue( context.get_abstract_state() is context.get_mutable_abstract_state()) self.assertTrue( context.get_abstract_state(0) is context.get_mutable_abstract_state(0)) self.assertEquals( context.get_abstract_state(0).get_value(), model_value.get_value()) # Check AbstractValues API. values = context.get_abstract_state() self.assertEquals(values.size(), 1) self.assertEquals( values.get_value(0).get_value(), model_value.get_value()) self.assertEquals( values.get_mutable_value(0).get_value(), model_value.get_value()) values.CopyFrom(values.Clone()) # - Check diagram context accessors. builder = DiagramBuilder() builder.AddSystem(system) diagram = builder.Build() context = diagram.CreateDefaultContext() # Existence check. self.assertTrue( diagram.GetMutableSubsystemState(system, context) is not None) self.assertTrue( diagram.GetMutableSubsystemContext(system, context) is not None)
def __init__(self, hand_controller, hand_plant): LeafSystem.__init__(self) self.hand_controller = hand_controller self.hand_plant = hand_plant self.n_cf = len(hand_controller.grasp_points) self._data = [] self._sample_times = np.empty((0, 1)) self.shut_up = False # Contact results # self.DeclareInputPort('contact_results', PortDataType.kAbstractValued, # hand_plant.contact_results_output_port().size()) self.DeclareAbstractInputPort("contact_results", AbstractValue.Make(mut.ContactResults()))
def test_no_rgb(self): self.pc_concat.GetInputPort("point_cloud_CiSi_0").FixValue( self.context, AbstractValue.Make(self.pc_no_rgbs)) self.pc_concat.GetInputPort("point_cloud_CiSi_1").FixValue( self.context, AbstractValue.Make(self.pc_no_rgbs)) fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval( self.context) self.assertEqual(fused_pc.size(), 2 * self.num_points) # The first point cloud should be from [-0.1 to 0.1]. # The second point cloud should be from [0.9 to 1.1]. self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0) self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0) # Even if both input point clouds don't have rgbs, the fused point # cloud should contain rgbs of the default color. self.assertTrue(fused_pc.has_rgbs()) self.assertTrue( np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255]))) self.assertTrue( np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])))
def test_rgb(self): self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(), AbstractValue.Make(self.pc)) self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(), AbstractValue.Make(self.pc)) fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval( self.context) self.assertEqual(fused_pc.size(), 2 * self.num_points) # The first point cloud should be from [-0.1 to 0.1]. # The second point cloud should be from [0.9 to 1.1]. self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0) self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0) self.assertTrue(fused_pc.has_rgbs()) self.assertTrue( np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255]))) self.assertTrue( np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255])))
def test_context_fix_input_port(self): # WARNING: This is not the recommend workflow; instead, use # `InputPort.FixValue` instead. This is here just for testing / # coverage purposes. dt = 0.1 # Arbitrary. system_vec = ZeroOrderHold(period_sec=dt, vector_size=1) context_vec = system_vec.CreateDefaultContext() context_vec.FixInputPort(index=0, data=[0.]) context_vec.FixInputPort(index=0, vec=BasicVector([0.])) # Test abstract. model_value = AbstractValue.Make("Hello") system_abstract = ZeroOrderHold( period_sec=dt, abstract_model_value=model_value.Clone()) context_abstract = system_abstract.CreateDefaultContext() context_abstract.FixInputPort(index=0, value=model_value.Clone())
def test_abstract_input_port_eval(self): model_value = AbstractValue.Make("Hello World") system = PassThrough(copy.copy(model_value)) context = system.CreateDefaultContext() fixed = context.FixInputPort(0, copy.copy(model_value)) self.assertIsInstance(fixed.GetMutableData(), AbstractValue) input_port = system.get_input_port(0) value = input_port.Eval(context) self.assertEqual(type(value), type(model_value.get_value())) self.assertEqual(value, model_value.get_value()) value_abs = input_port.EvalAbstract(context) self.assertEqual(type(value_abs), type(model_value)) self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_subscriber_wait_for_message(self): """Ensures that `WaitForMessage` threading works in a Python workflow that is not threaded.""" # N.B. This will fail with `threading`. See below for using # `multithreading`. lcm = DrakeLcm("memq://") lcm.StartReceiveThread() sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm) value = AbstractValue.Make(header_t()) for i in range(3): message = header_t() message.utime = i lcm.Publish("TEST_LOOP", message.encode()) sub.WaitForMessage(i, value) self.assertEqual(value.get_value().utime, i)
def test_abstract_input_port_fix_object(self): # The port type is py::object, not any specific C++ type. model_value = AbstractValue.Make(object()) system = PassThrough(copy.copy(model_value)) context = system.CreateDefaultContext() input_port = system.get_input_port(0) # Fix to a type-erased py::object. input_port.FixValue(context, AbstractValue.Make(object())) # Fix to an int. input_port.FixValue(context, 1) value = input_port.Eval(context) self.assertEqual(type(value), int) self.assertEqual(value, 1) # Fixing to an explicitly-typed Value instantiation is an error ... with self.assertRaises(RuntimeError): input_port.FixValue(context, AbstractValue.Make("string")) # ... but implicit typing works just fine. input_port.FixValue(context, "string") value = input_port.Eval(context) self.assertEqual(type(value), str) self.assertEqual(value, "string")
def test_subscriber_wait_for_message(self): """Checks how `WaitForMessage` works without Python threads.""" lcm = DrakeLcm("memq://") sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm) value = AbstractValue.Make(header_t()) for old_message_count in range(3): message = header_t() message.utime = old_message_count + 1 lcm.Publish("TEST_LOOP", message.encode()) for attempt in range(10): new_count = sub.WaitForMessage( old_message_count, value, timeout=0.02) if new_count > old_message_count: break lcm.HandleSubscriptions(0) self.assertEqual(value.get_value().utime, old_message_count + 1)
def test_contact(self): # PenetrationAsContactPair point_pair = PenetrationAsPointPair() self.assertTrue(isinstance(point_pair.id_A, GeometryId)) self.assertTrue(isinstance(point_pair.id_B, GeometryId)) self.assertTrue(point_pair.p_WCa.shape == (3, )) self.assertTrue(point_pair.p_WCb.shape == (3, )) self.assertTrue(isinstance(point_pair.depth, float)) # PointPairContactInfo id_A = BodyIndex(0) id_B = BodyIndex(1) contact_info = PointPairContactInfo(bodyA_index=id_A, bodyB_index=id_B, f_Bc_W=np.array([0, 0, 1]), p_WC=np.array([0, 0, 0]), separation_speed=0, slip_speed=0, point_pair=point_pair) self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex)) self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex)) self.assertTrue(contact_info.contact_force().shape == (3, )) self.assertTrue(contact_info.contact_point().shape == (3, )) self.assertTrue(isinstance(contact_info.slip_speed(), float)) self.assertIsInstance(contact_info.point_pair(), PenetrationAsPointPair) # ContactResults contact_results = ContactResults() contact_results.AddContactInfo(contact_info) self.assertTrue(contact_results.num_contacts() == 1) self.assertTrue( isinstance(contact_results.contact_info(0), PointPairContactInfo)) # ContactResultsToLcmSystem file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() contact_results_to_lcm = ContactResultsToLcmSystem(plant) context = contact_results_to_lcm.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(contact_results)) output = contact_results_to_lcm.AllocateOutput() contact_results_to_lcm.CalcOutput(context, output) result = output.get_data(0) self.assertIsInstance(result, AbstractValue)
def test_point_cloud_api(self): self.assertEqual(mut.PointCloud.T, np.float32) self.assertEqual(mut.PointCloud.C, np.uint8) self.assertEqual(mut.PointCloud.D, np.float32) self.assertTrue(mut.PointCloud.IsDefaultValue( value=mut.PointCloud.kDefaultValue)) self.assertTrue(mut.PointCloud.IsInvalidValue( value=mut.PointCloud.kDefaultValue)) fields = mut.Fields(mut.BaseField.kXYZs) pc = mut.PointCloud(new_size=0, fields=fields) self.assertEqual(pc.fields(), fields) self.assertEqual(pc.size(), 0) pc.resize(new_size=2) self.assertEqual(pc.size(), 2) self.assertTrue(pc.has_xyzs()) self.assertEqual(pc.xyzs().shape, (3, 2)) # Test reference semantics with NumPy + Pybind11. test_xyzs = [[1., 2., 3.], [4., 5., 6.]] pc.mutable_xyzs().T[:] = test_xyzs np.testing.assert_equal(pc.xyzs().T, test_xyzs) test_xyz = [10., 20., 30.] pc.mutable_xyz(i=0)[:] = test_xyz np.testing.assert_equal(pc.xyz(i=0), test_xyz) np.testing.assert_equal(pc.xyzs().T[0], test_xyz) pc_new = mut.PointCloud() pc_new.SetFrom(other=pc) np.testing.assert_equal(pc.xyzs(), pc_new.xyzs()) # - Additional types are tested via simple existence checks. all_fields = mut.Fields( mut.BaseField.kXYZs | mut.BaseField.kNormals | mut.BaseField.kRGBs) count = 1 pc = mut.PointCloud(new_size=count, fields=all_fields) self.check_array(pc.mutable_xyzs(), np.float32, (3, count)) self.check_array(pc.mutable_normals(), np.float32, (3, count)) pc.normals() pc.mutable_normal(i=0) pc.normal(i=0) self.check_array(pc.mutable_rgbs(), np.uint8, (3, count)) pc.rgbs() pc.mutable_rgb(i=0) pc.rgb(i=0) # - Check for none. with self.assertRaises(RuntimeError) as ex: mut.PointCloud(new_size=0, fields=mut.Fields(mut.BaseField.kNone)) # Test Systems' value registration. self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
def __init__(self, scene_graph): LeafSystem.__init__(self) assert scene_graph mbp = MultibodyPlant(0.0) parser = Parser(mbp, scene_graph) model_id = parser.AddModelFromFile( FindResource("models/glider/glider.urdf")) mbp.Finalize() self.source_id = mbp.get_source_id() self.body_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[0]) self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[1]) self.DeclareVectorInputPort("state", BasicVector(7)) self.DeclareAbstractOutputPort( "geometry_pose", lambda: AbstractValue.Make(FramePoseVector()), self.OutputGeometryPose)
def __init__(self, station, control_period=0.005, print_period=0.5): LeafSystem.__init__(self) self.set_name("Iiwa Controller") self.current_plan = None # Stuff for iiwa control self.nu = 7 self.print_period = print_period self.last_print_time = -print_period self.control_period = control_period # create a multibodyplant containing the robot only, which is used for # jacobian calculations. self.plant_iiwa = station.get_controller_plant() self.tree_iiwa = self.plant_iiwa.tree() self.context_iiwa = self.plant_iiwa.CreateDefaultContext() self.l7_frame = self.plant_iiwa.GetFrameByName('iiwa_link_7') # Declare iiwa_position/torque_command publishing rate self._DeclarePeriodicPublish(control_period) # iiwa position input port self.iiwa_position_input_port = \ self._DeclareInputPort( "iiwa_position", PortDataType.kVectorValued, 7) # iiwa velocity input port self.iiwa_velocity_input_port = \ self._DeclareInputPort( "iiwa_velocity", PortDataType.kVectorValued, 7) # plan abstract input port self.plan_input_port = \ self._DeclareAbstractInputPort( "iiwa_plan", AbstractValue.Make(PlanBase())) # position and torque command output port # first 7 elements are position commands. # last 7 elements are torque commands. self.iiwa_position_command_output_port = \ self._DeclareVectorOutputPort("iiwa_position_and_torque_command", BasicVector(self.nu*2), self._CalcIiwaCommand)
def test_deprecated_abstract_input_port(self): # A system that takes a Value[object] on its input, and parses the # input value's first element to a float on its output. class ParseFloatSystem(LeafSystem_[float]): def __init__(self): LeafSystem_[float].__init__(self) self._DeclareAbstractInputPort("in") self._DeclareVectorOutputPort("out", BasicVector(1), self._Out) def _Out(self, context, y_data): py_obj = self.EvalAbstractInput(context, 0).get_value()[0] y_data.SetAtIndex(0, float(py_obj)) system = ParseFloatSystem() context = system.CreateDefaultContext() output = system.AllocateOutput() context.FixInputPort(0, AbstractValue.Make(["22.2"])) system.CalcOutput(context, output) self.assertEqual(output.get_vector_data(0).GetAtIndex(0), 22.2)
def test_abstract_io_port(self): test = self # N.B. Since this has trivial operations, we can test all scalar types. for T in [float, AutoDiffXd, Expression]: default_value = ("default", T(0.)) expected_input_value = ("input", T(np.pi)) expected_output_value = ("output", 2*T(np.pi)) class CustomAbstractSystem(LeafSystem_[T]): def __init__(self): LeafSystem_[T].__init__(self) self.input_port = self.DeclareAbstractInputPort( "in", AbstractValue.Make(default_value)) self.output_port = self.DeclareAbstractOutputPort( "out", lambda: AbstractValue.Make(default_value), self.DoCalcAbstractOutput, prerequisites_of_calc=set([ self.input_port.ticket()])) def DoCalcAbstractOutput(self, context, y_data): input_value = self.EvalAbstractInput( context, 0).get_value() # The allocator function will populate the output with # the "input" test.assertTupleEqual(input_value, expected_input_value) y_data.set_value(expected_output_value) test.assertTupleEqual(y_data.get_value(), expected_output_value) system = CustomAbstractSystem() context = system.CreateDefaultContext() self.assertEqual(context.num_input_ports(), 1) context.FixInputPort(0, AbstractValue.Make(expected_input_value)) output = system.AllocateOutput() self.assertEqual(output.num_ports(), 1) with catch_drake_warnings(expected_count=1): output.get_num_ports() system.CalcOutput(context, output) value = output.get_data(0) self.assertEqual(value.get_value(), expected_output_value)
def test_parameters_api(self): def compare(actual, expected): self.assertEquals(type(actual), type(expected)) if isinstance(actual, VectorBase): self.assertTrue( np.allclose(actual.get_value(), expected.get_value())) else: self.assertEquals(actual.get_value(), expected.get_value()) model_numeric = BasicVector([0.]) model_abstract = AbstractValue.Make("Hello") params = Parameters( numeric=[model_numeric.Clone()], abstract=[model_abstract.Clone()]) self.assertEquals(params.num_numeric_parameters(), 1) self.assertEquals(params.num_abstract_parameters(), 1) # Numeric. compare(params.get_numeric_parameter(index=0), model_numeric) compare(params.get_mutable_numeric_parameter(index=0), model_numeric) # WARNING: This will invalidate old references! params.set_numeric_parameters(params.get_numeric_parameters().Clone()) # Abstract. compare(params.get_abstract_parameter(index=0), model_abstract) compare(params.get_mutable_abstract_parameter(index=0), model_abstract) # WARNING: This will invalidate old references! params.set_abstract_parameters( params.get_abstract_parameters().Clone()) # WARNING: This may invalidate old references! params.SetFrom(copy.deepcopy(params)) # Test alternative constructors. ctor_test = [ Parameters(), Parameters(numeric=[model_numeric.Clone()]), Parameters(abstract=[model_abstract.Clone()]), Parameters( numeric=[model_numeric.Clone()], abstract=[model_abstract.Clone()]), Parameters(vec=model_numeric.Clone()), Parameters(value=model_abstract.Clone()), ]
def test_vector_output_port_eval(self): np_value = np.array([1., 2., 3.]) model_value = AbstractValue.Make(BasicVector(np_value)) source = ConstantVectorSource(np_value) context = source.CreateDefaultContext() output_port = source.get_output_port(0) value = output_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np_value) value_abs = output_port.EvalAbstract(context) self.assertEqual(type(value_abs), type(model_value)) self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray) np.testing.assert_equal(value_abs.get_value().get_value(), np_value) basic = output_port.EvalBasicVector(context) self.assertEqual(type(basic), BasicVector) self.assertEqual(type(basic.get_value()), np.ndarray) np.testing.assert_equal(basic.get_value(), np_value)
def __init__(self, kuka_plans, gripper_setpoint_list, update_period=0.05): LeafSystem.__init__(self) self.set_name("Plan Scheduler") assert len(kuka_plans) == len(gripper_setpoint_list) # Add a zero order hold to hold the current position of the robot kuka_plans.insert( 0, JointSpacePlanRelative(duration=3.0, delta_q=np.zeros(7))) gripper_setpoint_list.insert(0, 0.055) if len(kuka_plans) > 1: # Insert to the beginning of plan_list a plan that moves the robot from its # current position to plan_list[0].traj.value(0) kuka_plans.insert( 1, JointSpacePlanGoToTarget( duration=6.0, q_target=kuka_plans[1].traj.value(0).flatten())) gripper_setpoint_list.insert(0, 0.055) self.gripper_setpoint_list = gripper_setpoint_list self.kuka_plans_list = kuka_plans self.current_plan = None self.current_gripper_setpoint = None self.current_plan_idx = 0 # Output ports for plans and gripper setpoints self.iiwa_plan_output_port = self._DeclareAbstractOutputPort( "iiwa_plan", lambda: AbstractValue.Make(PlanBase()), self._GetCurrentPlan) self.hand_setpoint_output_port = \ self._DeclareVectorOutputPort( "gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput) self.gripper_force_limit_output_port = \ self._DeclareVectorOutputPort( "force_limit", BasicVector(1), self._CalcForceLimitOutput) self._DeclarePeriodicPublish(update_period)
def test_vector_input_port_eval(self): np_value = np.array([1., 2., 3.]) model_value = AbstractValue.Make(BasicVector(np_value)) system = PassThrough(len(np_value)) context = system.CreateDefaultContext() system.get_input_port(0).FixValue(context, np_value) input_port = system.get_input_port(0) value = input_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np_value) value_abs = input_port.EvalAbstract(context) self.assertEqual(type(value_abs), type(model_value)) self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray) np.testing.assert_equal(value_abs.get_value().get_value(), np_value) basic = input_port.EvalBasicVector(context) self.assertEqual(type(basic), BasicVector) self.assertEqual(type(basic.get_value()), np.ndarray) np.testing.assert_equal(basic.get_value(), np_value)
def __init__(self, update_period=0.05): LeafSystem.__init__(self) self.set_name("Plan Scheduler") self.current_plan = None self.current_gripper_setpoint = None self.end_time = 0 # Output ports for plans and gripper setpoints self.iiwa_plan_output_port = self._DeclareAbstractOutputPort( "iiwa_plan", lambda: AbstractValue.Make(PlanBase()), self._GetCurrentPlan) self.hand_setpoint_output_port = \ self._DeclareVectorOutputPort( "gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput) self.gripper_force_limit_output_port = \ self._DeclareVectorOutputPort( "force_limit", BasicVector(1), self._CalcForceLimitOutput) self._DeclarePeriodicPublish(update_period)
def test_context_api(self): # Capture miscellaneous functions not yet tested. model_value = AbstractValue.Make("Hello") model_vector = BasicVector([1., 2.]) class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareContinuousState(1) self.DeclareDiscreteState(2) self.DeclareAbstractState(model_value.Clone()) self.DeclareAbstractParameter(model_value.Clone()) self.DeclareNumericParameter(model_vector.Clone()) system = TrivialSystem() context = system.CreateDefaultContext() self.assertTrue(context.get_state() is context.get_mutable_state()) self.assertEqual(context.num_continuous_states(), 1) self.assertTrue(context.get_continuous_state_vector() is context.get_mutable_continuous_state_vector()) self.assertEqual(context.num_discrete_state_groups(), 1) self.assertTrue(context.get_discrete_state_vector() is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state().get_vector(0)) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state().get_vector(0)) self.assertEqual(context.num_abstract_states(), 1) self.assertTrue(context.get_abstract_state() is context.get_mutable_abstract_state()) self.assertTrue( context.get_abstract_state(0) is context.get_mutable_abstract_state(0)) self.assertEqual( context.get_abstract_state(0).get_value(), model_value.get_value()) # Check abstract state API (also test AbstractValues). values = context.get_abstract_state() self.assertEqual(values.size(), 1) self.assertEqual( values.get_value(0).get_value(), model_value.get_value()) self.assertEqual( values.get_mutable_value(0).get_value(), model_value.get_value()) values.SetFrom(values.Clone()) # Check parameter accessors. self.assertEqual(system.num_abstract_parameters(), 1) self.assertEqual( context.get_abstract_parameter(index=0).get_value(), model_value.get_value()) self.assertEqual(system.num_numeric_parameter_groups(), 1) np.testing.assert_equal( context.get_numeric_parameter(index=0).get_value(), model_vector.get_value()) # Check diagram context accessors. builder = DiagramBuilder() builder.AddSystem(system) diagram = builder.Build() context = diagram.CreateDefaultContext() # Existence check. self.assertIsNot(diagram.GetMutableSubsystemState(system, context), None) subcontext = diagram.GetMutableSubsystemContext(system, context) self.assertIsNot(subcontext, None) self.assertIs(diagram.GetSubsystemContext(system, context), subcontext)
def CreateDefaultValue(self): return AbstractValue.Make(self._lcm_type())
def __init__(self, scene_graph, draw_period=_DEFAULT_PUBLISH_PERIOD, prefix="drake", zmq_url="default", open_browser=None, frames_to_draw={}, frames_opacity=1., axis_length=0.15, axis_radius=0.006): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a url to connect to the visualizer. Use ``zmp_url="default"`` to the value obtained by running a single ``meshcat-server`` in another terminal. Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a specific address. open_browser: Set to True to open the meshcat browser url in your default web browser. The default value of None will open the browser iff a new meshcat server is created as a subprocess. Set to False to disable this. frames_to_draw: a dictionary describing which body frames to draw. It is keyed on the names of model instances, and the values are sets of the names of the bodies whose body frames are shown. For example, if we want to draw the frames of body "A" and "B" in model instance "1", then frames_to_draw is {"1": {"A", "B"}}. frames_opacity, axis_length and axis_radius are the opacity, length and radius of the coordinate axes to be drawn. Note: This call will not return until it connects to the ``meshcat-server``. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self.DeclarePeriodicPublish(draw_period, 0.0) self.draw_period = draw_period # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" elif zmq_url == "new": zmq_url = None if zmq_url is None and open_browser is None: open_browser = True # Set up meshcat. self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) # drawing coordinate frames self.frames_to_draw = frames_to_draw self.frames_opacity = frames_opacity self.axis_length = axis_length self.axis_radius = axis_radius
def __init__(self, scene_graph, draw_period=1. / 30, T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), xlim=[-1., 1], ylim=[-1, 1], facecolor=[1, 1, 1], use_random_colors=False, substitute_collocated_mesh_files=True, ax=None, show=None): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. T_VW: The view projection matrix from world to view coordinates. xlim: View limit into the scene. ylim: View limit into the scene. facecolor: Passed through to figure() and sets background color. Both color name strings and RGB triplets are allowed. Defaults to white. use_random_colors: If set to True, will render each body with a different color. (Multiple visual elements on the same body will be the same color.) substitute_collocated_mesh_files: If True, then a mesh file specified with an unsupported filename extension may be replaced by a file of the same base name in the same directory, but with a supported filename extension. Currently only .obj files are supported. ax: If supplied, the visualizer will draw onto those axes instead of creating a new set of axes. The visualizer will still change the view range and figure size of those axes. show: Opens a window during initialization / publish iff True. Default is None, which implies show=True unless matplotlib.get_backend() is 'template'. """ default_size = matplotlib.rcParams['figure.figsize'] scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0]) figsize = (default_size[0], default_size[0] * scalefactor) PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize, ax=ax, draw_period=draw_period, show=show) self.set_name('planar_multibody_visualizer') self._scene_graph = scene_graph self._T_VW = T_VW # Pose bundle (from SceneGraph) input port. # TODO(tehbelinda): Rename the `lcm_visualization` port to match # SceneGraph once its output port has been updated. See #12214. self._pose_bundle_port = self.DeclareAbstractInputPort( "lcm_visualization", AbstractValue.Make(PoseBundle(0))) self.ax.axis('equal') self.ax.axis('off') # Achieve the desired view limits. self.ax.set_xlim(xlim) self.ax.set_ylim(ylim) default_size = self.fig.get_size_inches() self.fig.set_size_inches(figsize[0], figsize[1]) # Populate body patches. self._build_body_patches(use_random_colors, substitute_collocated_mesh_files) # Populate the body fill list -- which requires doing most of a draw # pass, but with an ax.fill() command to initialize the draw patches. # After initialization, we can then use in-place replacement of vertex # positions. The body fill list stores the ax patch objects in the # order they were spawned (i.e. by body, and then by order of view_ # patches). Drawing the tree should update them by iterating over # bodies and patches in the same order. self._body_fill_dict = {} X_WB_initial = RigidTransform.Identity() for full_name in self._patch_Blist.keys(): patch_Wlist, view_colors = self._get_view_patches( full_name, X_WB_initial) self._body_fill_dict[full_name] = [] for patch_W, color in zip(patch_Wlist, view_colors): # Project the full patch the first time, to initialize a vertex # list with enough space for any possible convex hull of this # vertex set. patch_V = self._project_patch(patch_W) body_fill = self.ax.fill(patch_V[0, :], patch_V[1, :], zorder=0, edgecolor='k', facecolor=color, closed=True)[0] self._body_fill_dict[full_name].append(body_fill) # Then update the vertices for a more accurate initial draw. self._update_body_fill_verts(body_fill, patch_V)