def test_vector_pass_through(self): model_value = BasicVector([1., 2, 3]) system = PassThrough(model_value.size()) context = system.CreateDefaultContext() context.FixInputPort(0, model_value) output = system.AllocateOutput() input_eval = system.EvalVectorInput(context, 0) compare_value(self, input_eval, model_value) system.CalcOutput(context, output) output_value = output.get_vector_data(0) compare_value(self, output_value, model_value)
def test_basic_vector_double(self): # Test constructing vectors of sizes [0, 1, 2], and ensure that we can # construct from both lists and `np.array` objects with no ambiguity. for n in [0, 1, 2]: for wrap in [pass_through, np.array]: # Ensure that we can get vectors templated on double by # reference. expected_init = wrap([float(x) for x in range(n)]) expected_add = wrap([x + 1 for x in expected_init]) expected_set = wrap([x + 10 for x in expected_init]) value_data = BasicVector(expected_init) value = value_data.get_mutable_value() self.assertTrue(np.allclose(value, expected_init)) # Add value directly. # TODO(eric.cousineau): Determine if there is a way to extract # the pointer referred to by the buffer (e.g. `value.data`). value[:] += 1 self.assertTrue(np.allclose(value, expected_add)) self.assertTrue( np.allclose(value_data.get_value(), expected_add)) self.assertTrue( np.allclose(value_data.get_mutable_value(), expected_add)) # Set value from `BasicVector`. value_data.SetFromVector(expected_set) self.assertTrue(np.allclose(value, expected_set)) self.assertTrue( np.allclose(value_data.get_value(), expected_set)) self.assertTrue( np.allclose(value_data.get_mutable_value(), expected_set)) # Ensure we can construct from size. value_data = BasicVector(n) self.assertEqual(value_data.size(), n) # Ensure we can clone. value_copies = [ value_data.Clone(), copy.copy(value_data), copy.deepcopy(value_data), ] for value_copy in value_copies: self.assertTrue(value_copy is not value_data) self.assertEqual(value_data.size(), n)
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() context.FixInputPort(0, 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, rb_tree): LeafSystem.__init__(self) self.rb_tree = rb_tree self.num_position = self.rb_tree.get_num_positions() self.num_controlled_q_ = self.rb_tree.get_num_actuators() self.n = 0 # Input Port self.joint_state_results_port_index = self._DeclareInputPort( 'joint_state_results_port', PortDataType.kVectorValued, self.num_position * 2).get_index() # self.contact_results_port_index = self._DeclareAbstractInputPort('contact_results_port', # AbstractValue.Make(ContactResults)).get_index() # # self.torque_port_index = self._DeclareInputPort('joint_state_results_port', # PortDataType.kVectorValued, # self.num_position * 2).get_index() # Output Port self.com_outport_index = self._DeclareVectorOutputPort( 'state_output_port', BasicVector(3), self._OutputCOM).get_index()
def __init__(self, arms, limit, timestep=0.005): LeafSystem.__init__(self) self.limit = limit self.timestep = timestep self.arms = arms self.target_input_ports = {} self.setpoint_output_ports = {} for arm in self.arms: self.target_input_ports[arm] = {} self.setpoint_output_ports[arm] = {} self.target_input_ports[arm][ "position"] = self.DeclareVectorInputPort( f"{arm}_target", BasicVector(6), ) self.target_input_ports[arm][ "width"] = self.DeclareVectorInputPort( f"{arm}_width_target", BasicVector(1), ) if "left" in self.arms: self.setpoint_output_ports["left"][ "position"] = self.DeclareVectorOutputPort( f"left_setpoint", BasicVector(6), self.DoCalcLeftOutput) self.setpoint_output_ports["left"][ "width"] = self.DeclareVectorOutputPort( f"left_width_setpoint", BasicVector(1), self.DoCalcLeftWidthOutput) if "right" in self.arms: self.setpoint_output_ports["right"][ "position"] = self.DeclareVectorOutputPort( f"right_setpoint", BasicVector(6), self.DoCalcRightOutput) self.setpoint_output_ports["right"][ "width"] = self.DeclareVectorOutputPort( f"right_width_setpoint", BasicVector(1), self.DoCalcRightWidthOutput) self.current_states = {} for arm in self.arms: self.current_states[arm] = {} self.current_states[arm]["position"] = self.DeclareDiscreteState(6) self.current_states[arm]["width"] = self.DeclareDiscreteState(1) self.DeclarePeriodicDiscreteUpdate(self.timestep)
def test_linear_quadratic_regulator(self): A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [1]]) C = np.identity(2) D = np.array([[0], [0]]) double_integrator = LinearSystem(A, B, C, D) Q = np.identity(2) R = np.identity(1) K_expected = np.array([[1, math.sqrt(3.)]]) S_expected = np.array([[math.sqrt(3), 1.], [1., math.sqrt(3)]]) (K, S) = LinearQuadraticRegulator(A, B, Q, R) np.testing.assert_almost_equal(K, K_expected) np.testing.assert_almost_equal(S, S_expected) controller = LinearQuadraticRegulator(double_integrator, Q, R) np.testing.assert_almost_equal(controller.D(), -K_expected) context = double_integrator.CreateDefaultContext() context.FixInputPort(0, BasicVector([0])) controller = LinearQuadraticRegulator(double_integrator, context, Q, R) np.testing.assert_almost_equal(controller.D(), -K_expected)
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_continuous = False self.called_discrete = False self.called_initialize = False # Ensure we have desired overloads. self._DeclarePeriodicPublish(0.1) self._DeclarePeriodicPublish(0.1, 0) self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.) self._DeclarePeriodicDiscreteUpdate( period_sec=0.1, offset_sec=0.) self._DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self._DeclareContinuousState(2) self._DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self, arm_info, timestep=0.0005): LeafSystem.__init__(self) self.set_name("low_level_hand_controller") self.arm_info = arm_info self.previous_error = {} self.integral = {} self.desired_input_ports = {} self.estimated_input_ports = {} self.body_positions_input_port = self.DeclareAbstractInputPort( "body_positions", Value[List[RigidTransform]]()) for arm in self.arm_info: self.desired_input_ports[arm] = self.DeclareVectorInputPort( f"{arm}_desired", BasicVector(6)) self.previous_error[arm] = self.DeclareDiscreteState(6) self.integral[arm] = self.DeclareDiscreteState(6) self.DeclareAbstractOutputPort( f"spatial_forces_vector", lambda: Value[List[ExternallyAppliedSpatialForce]](), self.DoCalcAbstractOutput) self.kp = [100, 100, 100, 10000, 10000, 10000] self.ki = [0, 0, 0, 1, 1, 1] self.kd = [10, 10, 10, 10, 10, 10] self.timestep = timestep
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 __init__(self, kuka_plans, gripper_setpoint_list, control_period=0.005, print_period=0.5): LeafSystem.__init__(self) assert len(kuka_plans) == len(gripper_setpoint_list) self.set_name("Manipulation Plan Runner") # 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_start_time = 0. self.current_plan = None self.current_gripper_setpoint = None self.current_plan_idx = 0 # Stuff for iiwa control self.nu = 7 self.print_period = print_period self.last_print_time = -print_period self.control_period = 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) # iiwa external torque input port self.iiwa_external_torque_input_port = \ self._DeclareInputPort( "iiwa_torque_external", PortDataType.kVectorValued, 7) # position and torque command output port self.iiwa_position_command_output_port = \ self._DeclareVectorOutputPort("iiwa_position_command", BasicVector(self.nu), self._CalcIiwaPositionCommand) self.iiwa_torque_command_output_port = \ self._DeclareVectorOutputPort("iiwa_torque_command", BasicVector(self.nu), self._CalcIiwaTorqueCommand) # gripper setpoint and torque limit self.hand_setpoint_output_port = \ self._DeclareVectorOutputPort( "gripper_setpoint", BasicVector(1), self._CalcGripperSetpointOutput) self.gripper_force_limit_output_port = \ self._DeclareVectorOutputPort( "force_limit", BasicVector(1), self._CalcForceLimitOutput) # Declare command publishing rate # state[0:7]: position command # state[7:14]: torque command # state[14]: gripper_setpoint self._DeclareDiscreteState(self.nu*2 + 1) self._DeclarePeriodicDiscreteUpdate(period_sec=self.control_period) self.kPlanDurationMultiplier = 1.1
xyz=[-2, 0, 2], # Ensure that the box is within range. rpy=[0, np.pi / 4, 0]) tree.addFrame(frame) # Create camera. camera = RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=True) # - Describe state. x = np.zeros(tree.get_num_positions() + tree.get_num_velocities()) # Allocate context and render. context = camera.CreateDefaultContext() context.FixInputPort(0, BasicVector(x)) output = camera.AllocateOutput(context) camera.CalcOutput(context, output) # Get images from computed output. color_index = camera.color_image_output_port().get_index() color_image = output.get_data(color_index).get_value() color_array = color_image.data depth_index = camera.depth_image_output_port().get_index() depth_image = output.get_data(depth_index).get_value() depth_array = depth_image.data # Show camera info and images. print("Intrinsics:\n{}".format(camera.depth_camera_info().intrinsic_matrix())) dpi = mpl.rcParams['figure.dpi']
def __init__(self): LeafSystem.__init__(self) self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6), self._DoCalcOutput) # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self._DeclarePeriodicPublish(0.01, 0.0) self.window = tk.Tk() self.window.title("End-Effector TeleOp") self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="roll", length=800, orient=tk.HORIZONTAL) self.roll.pack() self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="pitch", length=800, orient=tk.HORIZONTAL) self.pitch.pack() self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="yaw", length=800, orient=tk.HORIZONTAL) self.yaw.pack() self.x = tk.Scale(self.window, from_=-0.6, to=0.8, resolution=-1, label="x", length=800, orient=tk.HORIZONTAL) self.x.pack() self.y = tk.Scale(self.window, from_=-0.8, to=0.3, resolution=-1, label="y", length=800, orient=tk.HORIZONTAL) self.y.pack() self.z = tk.Scale(self.window, from_=0, to=1.1, resolution=-1, label="z", length=800, orient=tk.HORIZONTAL) self.z.pack() # The key bindings below provide teleop functionality via the # keyboard, and are somewhat arbitrary (inspired by gaming # conventions). Note that in order for the keyboard bindings to # be active, the teleop slider window must be the active window. def update(scale, value): return lambda event: scale.set(scale.get() + value) # Delta displacements for motion via keyboard teleop. rotation_delta = 0.05 # rad position_delta = 0.01 # m # Linear motion key bindings. self.window.bind("<Up>", update(self.z, +position_delta)) self.window.bind("<Down>", update(self.z, -position_delta)) self.window.bind("<d>", update(self.y, +position_delta)) self.window.bind("<a>", update(self.y, -position_delta)) self.window.bind("<w>", update(self.x, +position_delta)) self.window.bind("<s>", update(self.x, -position_delta)) # Rotational motion key bindings. self.window.bind("<Control-d>", update(self.pitch, +rotation_delta)) self.window.bind("<Control-a>", update(self.pitch, -rotation_delta)) self.window.bind("<Control-w>", update(self.roll, +rotation_delta)) self.window.bind("<Control-s>", update(self.roll, -rotation_delta)) self.window.bind("<Control-Up>", update(self.yaw, +rotation_delta)) self.window.bind("<Control-Down>", update(self.yaw, -rotation_delta))
def __init__(self): LeafSystem.__init__(self) self.DeclareAbstractInputPort("header_t", AbstractValue.Make(header_t)) self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def test_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.0]) plant.Finalize() # We verify the (known) size of the model. kNumPositions = 7 kNumVelocities = 7 kNumActuators = 7 kStateSize = kNumPositions + kNumVelocities self.assertEqual(plant.num_positions(), kNumPositions) self.assertEqual(plant.num_velocities(), kNumVelocities) self.assertEqual(plant.num_actuators(), kNumActuators) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), kNumVelocities) self.assertEqual( controller.get_input_port(estimated_state_port).size(), kStateSize) self.assertEqual( controller.get_input_port(desired_state_port).size(), kStateSize) self.assertEqual( controller.get_output_port(control_port).size(), kNumVelocities) # Current state. q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # Reference state and acceleration. q_r = q + 0.1 * np.ones_like(q) v_r = v + 0.1 * np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp * (q_r - q) + kd * (v_r - v) + ki * integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # Set the plant's context. plant_context = plant.CreateDefaultContext() x_plant = plant.GetMutablePositionsAndVelocities(plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.CalcInverseDynamics(plant_context, vd_d, MultibodyForces(plant)) # Verify the result. controller.CalcOutput(context, output) self.assertTrue( np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
def test_diagram_simulation(self): # Similar to: //systems/framework:diagram_test, ExampleDiagram size = 3 builder = DiagramBuilder() adder0 = builder.AddSystem(Adder(2, size)) adder0.set_name("adder0") adder1 = builder.AddSystem(Adder(2, size)) adder1.set_name("adder1") integrator = builder.AddSystem(Integrator(size)) integrator.set_name("integrator") builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0)) builder.Connect(adder1.get_output_port(0), integrator.get_input_port(0)) builder.ExportInput(adder0.get_input_port(0)) builder.ExportInput(adder0.get_input_port(1)) builder.ExportInput(adder1.get_input_port(1)) builder.ExportOutput(integrator.get_output_port(0)) diagram = builder.Build() # TODO(eric.cousineau): Figure out unicode handling if needed. # See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73) # for an example name. diagram.set_name("test_diagram") simulator = Simulator(diagram) context = simulator.get_mutable_context() # Create and attach inputs. # TODO(eric.cousineau): Not seeing any assertions being printed if no # inputs are connected. Need to check this behavior. input0 = np.array([0.1, 0.2, 0.3]) context.FixInputPort(0, input0) input1 = np.array([0.02, 0.03, 0.04]) context.FixInputPort(1, input1) input2 = BasicVector([0.003, 0.004, 0.005]) context.FixInputPort(2, input2) # Test the BasicVector overload. # Initialize integrator states. integrator_xc = ( diagram.GetMutableSubsystemState(integrator, context) .get_mutable_continuous_state().get_vector()) integrator_xc.SetFromVector([0, 1, 2]) simulator.Initialize() # Simulate briefly, and take full-context snapshots at intermediate # points. n = 6 times = np.linspace(0, 1, n) context_log = [] for t in times: simulator.StepTo(t) # Record snapshot of *entire* context. context_log.append(context.Clone()) xc_initial = np.array([0, 1, 2]) xc_final = np.array([0.123, 1.234, 2.345]) for i, context_i in enumerate(context_log): t = times[i] self.assertEqual(context_i.get_time(), t) xc = context_i.get_continuous_state_vector().CopyToVector() xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) + xc_initial) print("xc[t = {}] = {}".format(t, xc)) self.assertTrue(np.allclose(xc, xc_expected))
def __init__(self, plant, contacts_per_frame, is_wbc=False): LeafSystem.__init__(self) self.plant = plant self.contacts_per_frame = contacts_per_frame self.is_wbc = is_wbc self.upright_context = self.plant.CreateDefaultContext() self.q_nom = self.plant.GetPositions( self.upright_context) # Nominal upright pose self.input_q_v_idx = self.DeclareVectorInputPort( "q_v", BasicVector(self.plant.num_positions() + self.plant.num_velocities())).get_index() self.output_tau_idx = self.DeclareVectorOutputPort( "tau", BasicVector(Atlas.NUM_ACTUATED_DOF), self.calcTorqueOutput).get_index() if self.is_wbc: com_dim = 3 self.x_size = 2 * com_dim self.u_size = com_dim self.input_r_des_idx = self.DeclareVectorInputPort( "r_des", BasicVector(com_dim)).get_index() self.input_rd_des_idx = self.DeclareVectorInputPort( "rd_des", BasicVector(com_dim)).get_index() self.input_rdd_des_idx = self.DeclareVectorInputPort( "rdd_des", BasicVector(com_dim)).get_index() self.input_q_des_idx = self.DeclareVectorInputPort( "q_des", BasicVector(self.plant.num_positions())).get_index() self.input_v_des_idx = self.DeclareVectorInputPort( "v_des", BasicVector(self.plant.num_velocities())).get_index() self.input_vd_des_idx = self.DeclareVectorInputPort( "vd_des", BasicVector(self.plant.num_velocities())).get_index() Q = 1.0 * np.identity(self.x_size) R = 0.1 * np.identity(self.u_size) A = np.vstack([ np.hstack([0 * np.identity(com_dim), 1 * np.identity(com_dim)]), np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)]) ]) B_1 = np.vstack( [0 * np.identity(com_dim), 1 * np.identity(com_dim)]) K, S = LinearQuadraticRegulator(A, B_1, Q, R) def V_full(x, u, r, rd, rdd): x_bar = x - np.concatenate([r, rd]) u_bar = u - rdd ''' xd_bar = d(x - [r, rd].T)/dt = xd - [rd, rdd].T = Ax + Bu - [rd, rdd].T ''' xd_bar = A.dot(x) + B_1.dot(u) - np.concatenate([rd, rdd]) return x_bar.T.dot(Q).dot(x_bar) + u_bar.T.dot(R).dot( u_bar) + 2 * x_bar.T.dot(S).dot(xd_bar) self.V_full = V_full else: # Only x, y coordinates of COM is considered com_dim = 2 self.x_size = 2 * com_dim self.u_size = com_dim self.input_y_des_idx = self.DeclareVectorInputPort( "y_des", BasicVector(zmp_state_size)).get_index() ''' Eq(1) ''' A = np.vstack([ np.hstack([0 * np.identity(com_dim), 1 * np.identity(com_dim)]), np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)]) ]) B_1 = np.vstack( [0 * np.identity(com_dim), 1 * np.identity(com_dim)]) ''' Eq(4) ''' C_2 = np.hstack([np.identity(2), np.zeros((2, 2))]) # C in Eq(2) D = -z_com / Atlas.g * np.identity(zmp_state_size) Q = 1.0 * np.identity(zmp_state_size) ''' Eq(6) ''' ''' y.T*Q*y = (C*x+D*u)*Q*(C*x+D*u).T = x.T*C.T*Q*C*x + u.T*D.T*Q*D*u + x.T*C.T*Q*D*u + u.T*D.T*Q*C*X = .. + 2*x.T*C.T*Q*D*u ''' K, S = LinearQuadraticRegulator(A, B_1, C_2.T.dot(Q).dot(C_2), D.T.dot(Q).dot(D), C_2.T.dot(Q).dot(D)) # Use original formulation def V_full(x, u, y_des): # Assume constant z_com, we don't need tvLQR y = C_2.dot(x) + D.dot(u) def dJ_dx(x): return x.T.dot( S.T + S ) # https://math.stackexchange.com/questions/20694/vector-derivative-w-r-t-its-transpose-fracdaxdxt y_bar = y - y_des x_bar = x - np.concatenate( [y_des, [0.0, 0.0]]) # FIXME: This doesn't seem right... # FIXME: xd_bar should depend on yd_des xd_bar = A.dot(x_bar) + B_1.dot(u) return y_bar.T.dot(Q).dot(y_bar) + dJ_dx(x_bar).dot(xd_bar) self.V_full = V_full # Calculate values that don't depend on context self.B_7 = self.plant.MakeActuationMatrix() # From np.sort(np.nonzero(B_7)[0]) we know that indices 0-5 are the unactuated 6 DOF floating base and 6-35 are the actuated 30 DOF robot joints self.v_idx_act = 6 # Start index of actuated joints in generalized velocities self.B_a = self.B_7[self.v_idx_act:, :] self.B_a_inv = np.linalg.inv(self.B_a) # Sort joint effort limits to be the same order as tau in Eq(13) self.sorted_max_efforts = np.array([ entry[1].effort for entry in getJointLimitsSortedByActuator(self.plant) ])
def __init__(self, num_inputs, size): LeafSystem.__init__(self) for i in range(num_inputs): self.DeclareVectorInputPort("input{}".format(i), BasicVector(size)) self.DeclareVectorOutputPort("sum", BasicVector(size), self._calc_sum)
def __init__(self): LeafSystem.__init__(self) self.called_publish = False # Check a non-overridable method with catch_drake_warnings(expected_count=1): self._DeclareVectorInputPort("x", BasicVector(1))
def __init__(self, mbp, symbol_list, primitive_list, dfa_json_file, update_period=0.1): LeafSystem.__init__(self) self.mbp = mbp # Our version of MBP context, which we'll modify # in the publish method. self.mbp_context = mbp.CreateDefaultContext() self.set_name('task_execution_system') # Take robot state vector as input. self.DeclareVectorInputPort( "mbp_state_vector", BasicVector(mbp.num_positions() + mbp.num_velocities())) # State of system is current RPYXYZ and gripper goals self.DeclareAbstractState(AbstractValue.Make(self.TESState())) self.DeclarePeriodicEvent(period_sec=update_period, offset_sec=0.0, event=UnrestrictedUpdateEvent( self.UpdateAbstractState)) # Produce RPYXYZ goals + gripper goals self.DeclareVectorOutputPort("rpy_xyz_desired", BasicVector(6), self.CopyRpyXyzDesiredOut) self.DeclareVectorOutputPort("wsg_position", BasicVector(1), self.CopyWsgPositionOut) # Load the JSON file with open(dfa_json_file, 'r') as f: json_data = json.load(f) # Figure out which states in the JSON dict are environment # symbols, and which are primitives that we can execute. self.environment_symbol_indices = [] self.environment_symbols = [] self.action_primitive_indices = [] self.action_primitives = [] for var_i, var_name in enumerate(json_data["variables"]): # Split into symbols and primitives. found_symbol = None found_primitive = None for symbol in symbol_list: if symbol.name == var_name: if found_symbol is not None: raise ValueError( "Ambiguous matching symbol names provided for symbol {}." .format(var_name)) found_symbol = symbol for primitive in primitive_list: if primitive.name == var_name: if found_primitive is not None: raise ValueError( "Ambiguous matching primitive names provided for symbol {}." .format(var_name)) found_primitive = primitive if found_primitive and found_symbol: raise ValueError( "Both matching symbol AND primitive found for symbol {}.". format(var_name)) if not found_primitive and not found_symbol: raise ValueError( "No matching symbol or primitive found for symbol {}.". format(var_name)) if found_symbol is not None: self.environment_symbol_indices.append(var_i) self.environment_symbols.append(found_symbol) elif found_primitive: self.action_primitive_indices.append(var_i) self.action_primitives.append(found_primitive) # And now build the ultimate lookup dictionary containing, # at each node name: # ( [environment symbol truth vector for this state], # [possibly-empty list of primitives that can be taken, # [next state names]] ) self.state_lookup_table = {} for node_name in json_data["nodes"].keys(): node_symbols = [] node_state = np.array(json_data["nodes"][node_name]["state"]) next_node_names = [ str(x) for x in json_data["nodes"][node_name]["trans"] ] node_primitives = [] for i, prim in zip(self.action_primitive_indices, self.action_primitives): if node_state[i]: node_primitives.append(prim) self.state_lookup_table[node_name] = ( node_state[self.environment_symbol_indices], node_primitives, next_node_names)
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) with catch_drake_warnings(expected_count=1): context.get_num_discrete_state_groups() 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) with catch_drake_warnings(expected_count=1): context.get_num_abstract_states() 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()) with catch_drake_warnings(expected_count=1): values.CopyFrom(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 test_basic_vector_double(self): # Test constructing vectors of sizes [0, 1, 2], and ensure that we can # construct from both lists and `np.array` objects with no ambiguity. for n in [0, 1, 2]: for wrap in [pass_through, np.array]: # Ensure that we can get vectors templated on double by # reference. expected_init = wrap(map(float, range(n))) expected_add = wrap([x + 1 for x in expected_init]) expected_set = wrap([x + 10 for x in expected_init]) value_data = BasicVector(expected_init) value = value_data.get_mutable_value() self.assertTrue(np.allclose(value, expected_init)) # Add value directly. # TODO(eric.cousineau): Determine if there is a way to extract # the pointer referred to by the buffer (e.g. `value.data`). value[:] += 1 self.assertTrue(np.allclose(value, expected_add)) self.assertTrue( np.allclose(value_data.get_value(), expected_add)) self.assertTrue( np.allclose(value_data.get_mutable_value(), expected_add)) # Set value from `BasicVector`. value_data.SetFromVector(expected_set) self.assertTrue(np.allclose(value, expected_set)) self.assertTrue( np.allclose(value_data.get_value(), expected_set)) self.assertTrue( np.allclose(value_data.get_mutable_value(), expected_set)) # Ensure we can construct from size. value_data = BasicVector(n) self.assertEquals(value_data.size(), n) # Ensure we can clone. value_copies = [ value_data.Clone(), copy.copy(value_data), copy.deepcopy(value_data), ] for value_copy in value_copies: self.assertTrue(value_copy is not value_data) self.assertEquals(value_data.size(), n)
def __init__(self, num_inputs, size): LeafSystem.__init__(self) for i in xrange(num_inputs): self._DeclareInputPort(PortDataType.kVectorValued, size) self._DeclareVectorOutputPort(BasicVector(size), self._calc_sum)
def test_basic_vector_set_get(self): value = BasicVector(np.arange(3., 5.)) self.assertEqual(value.GetAtIndex(1), 4.) value.SetAtIndex(1, 5.) self.assertEqual(value.GetAtIndex(1), 5.)
def _fix_adder_inputs(self, context): self.assertEqual(context.num_input_ports(), 2) with catch_drake_warnings(expected_count=1): context.get_num_input_ports() context.FixInputPort(0, BasicVector([1, 2, 3])) context.FixInputPort(1, BasicVector([4, 5, 6]))
def __init__(self, planar=False): """ @param planar if True, restricts the GUI and the output to have y=0, roll=0, yaw=0. """ LeafSystem.__init__(self) self.DeclareVectorOutputPort("rpy_xyz", BasicVector(6), self.DoCalcOutput) # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self.DeclarePeriodicPublish(0.01, 0.0) self.planar = planar self.window = tk.Tk() self.window.title("End-Effector TeleOp") self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="roll (keys: ctrl-right, ctrl-left)", length=800, orient=tk.HORIZONTAL) self.roll.pack() self.roll.set(0) self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="pitch (keys: ctrl-d, ctrl-a)", length=800, orient=tk.HORIZONTAL) if not planar: self.pitch.pack() self.pitch.set(0) self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="yaw (keys: ctrl-up, ctrl-down)", length=800, orient=tk.HORIZONTAL) if not planar: self.yaw.pack() self.yaw.set(1.57) self.x = tk.Scale(self.window, from_=-0.6, to=0.8, resolution=-1, label="x (keys: right, left)", length=800, orient=tk.HORIZONTAL) self.x.pack() self.x.set(0) self.y = tk.Scale(self.window, from_=-0.8, to=0.3, resolution=-1, label="y (keys: d, a)", length=800, orient=tk.HORIZONTAL) if not planar: self.y.pack() self.y.set(0) self.z = tk.Scale(self.window, from_=0, to=1.1, resolution=-1, label="z (keys: up, down)", length=800, orient=tk.HORIZONTAL) self.z.pack() self.z.set(0) # The key bindings below provide teleop functionality via the # keyboard, and are somewhat arbitrary (inspired by gaming # conventions). Note that in order for the keyboard bindings to # be active, the teleop slider window must be the active window. def update(scale, value): return lambda event: scale.set(scale.get() + value) # Delta displacements for motion via keyboard teleop. rotation_delta = 0.05 # rad position_delta = 0.01 # m # Linear motion key bindings. self.window.bind("<Up>", update(self.z, +position_delta)) self.window.bind("<Down>", update(self.z, -position_delta)) if (not planar): self.window.bind("<d>", update(self.y, +position_delta)) self.window.bind("<a>", update(self.y, -position_delta)) self.window.bind("<Right>", update(self.x, +position_delta)) self.window.bind("<Left>", update(self.x, -position_delta)) # Rotational motion key bindings. self.window.bind("<Control-Right>", update(self.roll, +rotation_delta)) self.window.bind("<Control-Left>", update(self.roll, -rotation_delta)) if (not planar): self.window.bind("<Control-d>", update(self.pitch, +rotation_delta)) self.window.bind("<Control-a>", update(self.pitch, -rotation_delta)) self.window.bind("<Control-Up>", update(self.yaw, +rotation_delta)) self.window.bind("<Control-Down>", update(self.yaw, -rotation_delta))
def mytest(input, expected): context.FixInputPort(0, BasicVector(input)) system.CalcOutput(context, output) self.assertTrue( np.allclose( output.get_vector_data(0).CopyToVector(), expected))
def test_diagram_simulation(self): # TODO(eric.cousineau): Move this to `analysis_test.py`. # Similar to: //systems/framework:diagram_test, ExampleDiagram size = 3 builder = DiagramBuilder() self.assertTrue(builder.empty()) adder0 = builder.AddSystem(Adder(2, size)) adder0.set_name("adder0") self.assertFalse(builder.empty()) adder1 = builder.AddSystem(Adder(2, size)) adder1.set_name("adder1") integrator = builder.AddSystem(Integrator(size)) integrator.set_name("integrator") self.assertEqual( builder.GetMutableSystems(), [adder0, adder1, integrator]) builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0)) builder.Connect(adder1.get_output_port(0), integrator.get_input_port(0)) # Exercise naming variants. builder.ExportInput(adder0.get_input_port(0)) builder.ExportInput(adder0.get_input_port(1), kUseDefaultName) builder.ExportInput(adder1.get_input_port(1), "third_input") builder.ExportOutput(integrator.get_output_port(0), "result") diagram = builder.Build() self.assertEqual(adder0.get_name(), "adder0") self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0) self.assertEqual( diagram.GetSystems(), [adder0, adder1, integrator]) # TODO(eric.cousineau): Figure out unicode handling if needed. # See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73) # for an example name. diagram.set_name("test_diagram") simulator = Simulator(diagram) context = simulator.get_mutable_context() # Create and attach inputs. # TODO(eric.cousineau): Not seeing any assertions being printed if no # inputs are connected. Need to check this behavior. input0 = np.array([0.1, 0.2, 0.3]) diagram.get_input_port(0).FixValue(context, input0) input1 = np.array([0.02, 0.03, 0.04]) diagram.get_input_port(1).FixValue(context, input1) # Test the BasicVector overload. input2 = BasicVector([0.003, 0.004, 0.005]) diagram.get_input_port(2).FixValue(context, input2) # Initialize integrator states. integrator_xc = ( diagram.GetMutableSubsystemState(integrator, context) .get_mutable_continuous_state().get_vector()) integrator_xc.SetFromVector([0, 1, 2]) simulator.Initialize() # Simulate briefly, and take full-context snapshots at intermediate # points. n = 6 times = np.linspace(0, 1, n) context_log = [] for t in times: simulator.AdvanceTo(t) # Record snapshot of *entire* context. context_log.append(context.Clone()) # Test binding for PrintSimulatorStatistics PrintSimulatorStatistics(simulator) xc_initial = np.array([0, 1, 2]) xc_final = np.array([0.123, 1.234, 2.345]) for i, context_i in enumerate(context_log): t = times[i] self.assertEqual(context_i.get_time(), t) xc = context_i.get_continuous_state_vector().CopyToVector() xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) + xc_initial) self.assertTrue(np.allclose(xc, xc_expected))
def _fix_adder_inputs(self, context): self.assertEqual(context.num_input_ports(), 2) context.FixInputPort(0, BasicVector([1, 2, 3])) context.FixInputPort(1, BasicVector([4, 5, 6]))
class _ArgHelper: """Provides information and functions to aid in interfacing a Python function with the Systems framework.""" def __init__(self, name, cls, scalar_as_vector): """Given a class (or type annotation), figure out the type (vector port, abstract port, or context time), the model value (for ports), and example value (for output inference).""" # Name can be overridden. self.name = name self._scalar_needs_conversion = False self._is_direct_type = False if isinstance(cls, VectorArg): self.type = PortDataType.kVectorValued self.model = BasicVector(cls._size) self.model.get_mutable_value()[:] = 0 self.example = self.model.get_value() elif BasicVector_.is_instantiation(cls): assert False, ( f"Must supply BasicVector_[] instance, not type: {cls}") elif BasicVector_.is_instantiation(type(cls)): self.type = PortDataType.kVectorValued self.model = cls self.example = self.model self._is_direct_type = True elif scalar_as_vector and cls in SCALAR_TYPES: self.type = PortDataType.kVectorValued self.model = BasicVector(1) self.model.get_mutable_value()[:] = 0 self.example = float() # Should this be smarter about the type? self._scalar_needs_conversion = True elif cls is ContextTimeArg: self.type = ContextTimeArg self.model = None self.example = float() else: self.type = PortDataType.kAbstractValued self.model, self.example = _get_abstract_model_and_example(cls) if self.model is self.example: self._is_direct_type = True def _squeeze(self, x): if self._scalar_needs_conversion: assert x.shape == (1, ), f"Bad input: {x}" return x.item(0) else: return x def _unsqueeze(self, x): if self._scalar_needs_conversion: return np.array([x]) else: return x def declare_input_eval(self, system): """Declares an input evaluation function. If a port is needed, will declare the port.""" if self.type is ContextTimeArg: return Context.get_time elif self.type == PortDataType.kAbstractValued: # DeclareInputPort does not work with kAbstractValued :( port = system.DeclareAbstractInputPort(name=self.name, model_value=self.model) if self._is_direct_type: return port.EvalAbstract else: return port.Eval else: port = system.DeclareVectorInputPort(name=self.name, model_vector=self.model) if self._is_direct_type: return port.EvalBasicVector else: return lambda context: self._squeeze(port.Eval(context)) def declare_output_port(self, system, calc): """Declares an output port on a given system.""" if self.type is ContextTimeArg: assert False, dedent(r"""\ ContextTimeArg is disallowed for output arguments. If needed, explicitly pass it through, e.g.: def context_time(t: ContextTimeArg): return t """) elif self.type == PortDataType.kAbstractValued: system.DeclareAbstractOutputPort(name=self.name, alloc=self.model.Clone, calc=calc) else: system.DeclareVectorOutputPort(name=self.name, model_value=self.model, calc=calc) def get_set_output_func(self): assert self.type is not ContextTimeArg if self.type == PortDataType.kAbstractValued: if self._is_direct_type: return lambda output, value: output.SetFrom(value) else: return lambda output, value: output.set_value(value) else: if self._is_direct_type: # TODO(eric.cousineau): Bind VectorBase.SetFrom(). return lambda output, value: output.SetFromVector(value. get_value()) else: return lambda output, value: output.SetFromVector( self._unsqueeze(value))
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 __init__(self): LeafSystem_[float].__init__(self) self._DeclareAbstractInputPort("in") self._DeclareVectorOutputPort("out", BasicVector(1), self._Out)
def __init__(self): LeafSystem_[float].__init__(self) with catch_drake_warnings(expected_count=1): self.DeclareAbstractInputPort("in") self.DeclareVectorOutputPort("out", BasicVector(1), self._Out)
def __init__(self, num_inputs, size): LeafSystem.__init__(self) for i in range(num_inputs): self._DeclareInputPort(kUseDefaultName, PortDataType.kVectorValued, size) self._DeclareVectorOutputPort("sum", BasicVector(size), self._calc_sum)