def __init__(self, station, control_period=0.005, print_period=0.5): LeafSystem.__init__(self) self.set_name("Manipulation Plan Runner") self.gripper_setpoint_list = [] self.kuka_plans_list = [] 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 self.plant = station.get_mutable_multibody_plant() self.tree = self.plant.tree() # get relative transforms between EE frame (wsg gripper) and iiwa_link_7 context_plant = self.plant.CreateDefaultContext() self.X_L7E = self.tree.CalcRelativeTransform( context_plant, frame_A=self.plant.GetFrameByName("iiwa_link_7"), frame_B=self.plant.GetFrameByName("body")) self.X_EEa = GetEndEffectorWorldAlignedFrame() # 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 # iiwa velocity input port self.iiwa_position_input_port = \ self._DeclareInputPort( "iiwa_position", PortDataType.kVectorValued, 7) self.iiwa_velocity_input_port = \ self._DeclareInputPort( "iiwa_velocity", PortDataType.kVectorValued, 7) # 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) # gripper control self._DeclareDiscreteState(1) self._DeclarePeriodicDiscreteUpdate(period_sec=0.1) 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)
def __init__(self, rbt, plant, qtraj): LeafSystem.__init__(self) self.set_name("Manipulation State Machine") self.qtraj = qtraj self.rbt = rbt self.nq = rbt.get_num_positions() self.plant = plant self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, rbt.get_num_positions() + rbt.get_num_velocities()) self._DeclareDiscreteState(1) self._DeclarePeriodicDiscreteUpdate(period_sec=0.001) self.kuka_setpoint_output_port = \ self._DeclareVectorOutputPort( BasicVector(rbt.get_num_positions() + rbt.get_num_velocities()), self._DoCalcKukaSetpointOutput) self.hand_setpoint_output_port = \ self._DeclareVectorOutputPort(BasicVector(1), self._DoCalcHandSetpointOutput) self._DeclarePeriodicPublish(0.01, 0.0)
def __init__(self, plant): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self._G = plant.GetBodyByName("body").body_frame() self._W = plant.world_frame() self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput)
def __init__(self, plant): LeafSystem.__init__(self) self.plant = plant self.plant_context = plant.CreateDefaultContext() self.iiwa = plant.GetModelInstanceByName("iiwa7") self.Paddle = plant.GetBodyByName("base_link") self.W = plant.world_frame() self.DeclareVectorInputPort("iiwa_pos_measured", BasicVector(7)) self.DeclareVectorInputPort("iiwa_velocity_estimated", BasicVector(7)) self.DeclareVectorInputPort("ball_pose", BasicVector(3)) self.DeclareVectorInputPort("ball_velocity", BasicVector(3)) self.DeclareVectorOutputPort("mirror_velocity", BasicVector(3), self.CalcOutput)
def __init__(self, plant): LeafSystem.__init__(self) self.plant = plant self.plant_context = plant.CreateDefaultContext() self.iiwa = plant.GetModelInstanceByName("iiwa7") self.P = plant.GetBodyByName("base_link").body_frame() self.W = plant.world_frame() self.DeclareVectorInputPort("paddle_desired_velocity", BasicVector(3)) self.DeclareVectorInputPort("paddle_desired_angular_velocity", BasicVector(3)) self.DeclareVectorInputPort("iiwa_pos_measured", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput) self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start() self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()
def __init__( self, plant, gripper_to_object_dist, T_world_objectPickup, T_world_prethrow, p_world_target, planned_launch_angle, height_thresh=0.03, launch_angle_thresh=0.1, dbg_state_prints=False # turn this to true to get some helfpul dbg prints ): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self.gripper = plant.GetBodyByName("body") self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.qdot_port = self.DeclareVectorInputPort("iiwa_velocity", BasicVector(7)) self.DeclareVectorOutputPort("wsg_position", BasicVector(1), self.CalcGripperTarget) # important pose info self.gripper_to_object_dist = gripper_to_object_dist self.T_world_objectPickup = T_world_objectPickup self.T_world_prethrow = T_world_prethrow self.p_world_target = p_world_target self.planned_launch_angle = planned_launch_angle self.height_thresh = height_thresh self.launch_angle_thresh = launch_angle_thresh # some constants self.GRIPPER_OPEN = np.array([0.5]) self.GRIPPER_CLOSED = np.array([0.0]) # states self.gripper_picked_up_object = False self.reached_prethrow = False self.at_or_passed_release = False # this helps make sure we're in the right state self.is_robot_stationary = False self.translation_history = np.zeros(shape=(10, 3)) self.translation_history_idx = 0 # determines gripper control based on above logic self.should_close_gripper = False self.dbg_state_prints = dbg_state_prints
def __init__(self, rbt_full, q_nom, world_builder): LeafSystem.__init__(self) self.set_name("Task Planner") self.rbt = rbt_full self.world_builder = world_builder self.q_nom = np.array( [-0.18, -1., 0.12, -1.89, 0.1, 1.3, 0.38, 0.0, 0.0, 1.5]) self.nq_full = rbt_full.get_num_positions() self.nv_full = rbt_full.get_num_velocities() self.nu_full = rbt_full.get_num_actuators() self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, self.nq_full + self.nv_full) self._DeclareDiscreteState(1) self._DeclarePeriodicDiscreteUpdate(period_sec=0.1) # TODO set default state somehow better. Requires new # bindings to override AllocateDiscreteState or something else. self.initialized = False self.current_primitive = IdlePrimitive(self.rbt, q_nom) self.kuka_setpoint = self._DoAllocKukaSetpointOutput() # Put these in arrays so we can more easily pass by reference into # CalcSetpointsOutput self.gripper_setpoint = np.array([0.]) self.knife_setpoint = np.array([np.pi / 2.]) # TODO The controller takes full state in, even though it only # controls the Kuka... that should be tidied up. self.kuka_setpoint_output_port = \ self._DeclareAbstractOutputPort( self._DoAllocKukaSetpointOutput, self._DoCalcKukaSetpointOutput) self.hand_setpoint_output_port = \ self._DeclareVectorOutputPort(BasicVector(1), self._DoCalcHandSetpointOutput) self.knife_setpoint_output_port = \ self._DeclareVectorOutputPort(BasicVector(1), self._DoCalcKnifeSetpointOutput) self._DeclarePeriodicPublish(0.01, 0.0) # Really stupid simple state self.current_target_object = None self.current_target_object_move_location = None self.do_cut_after_current_move = False
def __init__(self, plant, model_instance, control_period=0.001): LeafSystem.__init__(self) self.set_name("Hand Controller") gripper_model_index_idx = 1 self.max_force = 100. # gripper max closing / opening force self.plant = plant self.model_instance = model_instance # TODO: access actuation ports with model_instance index self.nu = plant.get_input_port(gripper_model_index_idx).size() self.nq = plant.num_positions() self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, plant.num_positions() + plant.num_velocities()) self.setpoint_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareDiscreteState(self.nu) self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) self._DeclareVectorOutputPort(BasicVector(self.nu), self._DoCalcVectorOutput)
def __init__(self, Q=None, R=None): super().__init__() self.DeclareInputPort('quadrotor state', PortDataType.kVectorValued, 12) self.DeclareVectorOutputPort('control', BasicVector(4), self.calculate_control) self.m = 0.775 self.L = 0.15 self.kF = 1.0 self.kM = 0.0245 self.I = np.array([[0.0015, 0, 0], [0, 0.0025, 0], [0, 0, 0.0035]]) self.g = 9.81 self.nominal_state = [ 0, ] * 12 if Q is None: self.Q = np.diag([10, 10, 10, 20, 20, 10, 10, 10, 10, 1, 1, 1]) else: self.Q = Q if R is None: self.R = np.diag([1, 1, 1, 1]) else: self.R = R (K, S) = self.get_LQR_params() self.K = K
def __init__(self): LeafSystem.__init__(self) self._DeclareInputPort(PortDataType.kVectorValued, n) self._DeclareVectorOutputPort(BasicVector(m), self._DoCalcVectorOutput) self._DeclareDiscreteState(m) # state of the controller system is u self._DeclarePeriodicDiscreteUpdate(period_sec=traj_specs.h) # update u every h seconds. self.is_plan_computed = False
def __init__(self, rbt, plant, control_period=0.001): LeafSystem.__init__(self) self.set_name("GuillotineController Controller") self.controlled_joint_names = ["knife_joint"] self.max_force = 100. self.controlled_inds, _ = kuka_utils.extract_position_indices( rbt, self.controlled_joint_names) self.nu = plant.get_input_port(2).size() self.nq = rbt.get_num_positions() self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, rbt.get_num_positions() + rbt.get_num_velocities()) self.setpoint_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareDiscreteState(self.nu) self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) self._DeclareVectorOutputPort(BasicVector(self.nu), self._DoCalcVectorOutput)
def __init__(self, rbt, plant, control_period=0.001): LeafSystem.__init__(self) self.set_name("Hand Controller") self.controlled_joint_names = [ "left_finger_sliding_joint", "right_finger_sliding_joint" ] self.max_force = 50. # gripper max closing / opening force self.controlled_inds, _ = kuka_utils.extract_position_indices( rbt, self.controlled_joint_names) self.nu = plant.get_input_port(1).size() self.nq = rbt.get_num_positions() self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, rbt.get_num_positions() + rbt.get_num_velocities()) self.setpoint_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareDiscreteState(self.nu) self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) self._DeclareVectorOutputPort(BasicVector(self.nu), self._DoCalcVectorOutput)
def __init__(self, builder, dt=5e-4, N=150, params=None, trj_decay=0.7, x_w_cov=1e-5, door_angle_ref=1.0, visualize=False): self.plant_derivs = MultibodyPlant(time_step=dt) parser = Parser(self.plant_derivs) self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs, parser, params=params) self.plant_derivs.Finalize() self.plant_derivs_context = self.plant_derivs.CreateDefaultContext() self.plant_derivs.get_actuation_input_port().FixValue( self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.]) null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.plant_derivs.GetInputPort("applied_generalized_force").FixValue( self.plant_derivs_context, null_force) self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=dt) parser = Parser(self.plant, scene_graph) self.iiwa, self.hinge, self.bushing = self.add_models(self.plant, parser, params=params) self.plant.Finalize( ) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor. self.meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url) self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls nq = self.plant.num_positions() nv = self.plant.num_velocities() self.n_x = nq + nv self.n_u = self.plant.num_actuators() self.n_y = self.plant.get_state_output_port(self.iiwa).size() self.N = N self.dt = dt self.decay = trj_decay self.V = 1e-2 * np.ones(self.n_y) self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv))) self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv))) self.x_w_cov = x_w_cov self.door_angle_ref = door_angle_ref self.q0 = np.array( [-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) #self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55]) self.x0 = np.concatenate((self.q0, np.zeros(nv))) self.door_index = None self.phi = {}
def __init__(self): LeafSystem.__init__(self) self._DeclareInputPort(PortDataType.kVectorValued, n) self._DeclareVectorOutputPort(BasicVector(m), self._DoCalcVectorOutput) self._DeclareDiscreteState(m) # state of the controller system is u self._DeclarePeriodicDiscreteUpdate( period_sec=0.005) # update u at 200Hz
def __init__(self, plant, kuka_model_instance, control_period=0.005, print_period=0.5): LeafSystem.__init__(self) self.set_name("Kuka Controller") self.plant = plant self.tree = plant.tree() self.print_period = print_period self.last_print_time = -print_period self.control_period = control_period self.model_instance = kuka_model_instance self.nu = len(get_model_actuators(plant, kuka_model_instance)) self.nq = plant.num_positions() v_dummy = np.arange(plant.num_velocities()) self.controlled_inds = self.tree.get_velocities_from_array( kuka_model_instance, v_dummy).astype(int) self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, plant.num_positions() + plant.num_velocities()) self.plan_input_port = \ self._DeclareInputPort(PortDataType.kAbstractValued, 0) self._DeclareDiscreteState(self.nu) self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) self._DeclareVectorOutputPort(BasicVector(self.nu), self._DoCalcVectorOutput)
def __init__(self, plant, kuka_plans, gripper_setpoint_list): LeafSystem.__init__(self) self.set_name("Manipulation State Machine") # Append plan_list with a plan that moves the robot from its current position to # plan_list[0].traj.value(0) kuka_plans.insert(0, JointSpacePlan()) gripper_setpoint_list.insert(0, 0.05) self.move_to_home_duration_sec = 10.0 kuka_plans[0].duration = self.move_to_home_duration_sec # Add a five-second zero order hold to hold the current position of the robot kuka_plans.insert(0, JointSpacePlan()) gripper_setpoint_list.insert(0, 0.05) self.zero_order_hold_duration_sec = 5.0 kuka_plans[0].duration = self.zero_order_hold_duration_sec assert len(kuka_plans) == len(gripper_setpoint_list) self.gripper_setpoint_list = gripper_setpoint_list self.kuka_plans_list = kuka_plans self.num_plans = len(kuka_plans) self.t_plan = np.zeros(self.num_plans + 1) self.t_plan[1] = self.zero_order_hold_duration_sec self.t_plan[2] = self.move_to_home_duration_sec + self.t_plan[1] for i in range(2, self.num_plans): self.t_plan[i + 1] = \ self.t_plan[i] + kuka_plans[i].get_duration() * 1.1 print self.t_plan self.nq = plant.num_positions() self.plant = plant self._DeclareDiscreteState(1) self._DeclarePeriodicDiscreteUpdate(period_sec=0.01) self.kuka_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.iiwa_position_input_port = \ self._DeclareInputPort("iiwa_position", PortDataType.kVectorValued, 7)
def __init__(self): LeafSystem.__init__(self) self._DeclareInputPort(PortDataType.kVectorValued, 1, "noise", RandomDistribution.kGaussian) self._DeclareContinuousState(1) self._DeclareVectorOutputPort(BasicVector(1), self.CopyStateOut)
def __init__(self, rbt, plant, control_period=0.033, print_period=0.5): LeafSystem.__init__(self) self.set_name("Instantaneous Kuka Controller") self.controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] self.controlled_inds, _ = kuka_utils.extract_position_indices( rbt, self.controlled_joint_names) # Extract the full-rank bit of B, and verify that it's full rank self.nq_reduced = len(self.controlled_inds) self.B = np.empty((self.nq_reduced, self.nq_reduced)) for k in range(self.nq_reduced): for l in range(self.nq_reduced): self.B[k, l] = rbt.B[self.controlled_inds[k], self.controlled_inds[l]] if np.linalg.matrix_rank(self.B) < self.nq_reduced: raise RuntimeError("The joint set specified is underactuated.") self.B_inv = np.linalg.inv(self.B) # Copy lots of stuff self.rbt = rbt self.nq = rbt.get_num_positions() self.plant = plant self.nu = plant.get_input_port(0).size() # hopefully matches if self.nu != self.nq_reduced: raise RuntimeError("plant input port not equal to number of controlled joints") self.print_period = print_period self.last_print_time = -print_period self.shut_up = False self.control_period = control_period self.robot_state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, rbt.get_num_positions() + rbt.get_num_velocities()) self.setpoint_input_port = \ self._DeclareAbstractInputPort( "setpoint_input_port", AbstractValue.Make(InstantaneousKukaControllerSetpoint())) self._DeclareDiscreteState(self.nu) self._DeclarePeriodicDiscreteUpdate(period_sec=control_period) self._DeclareVectorOutputPort( BasicVector(self.nu), self._DoCalcVectorOutput)
def __init__(self, num_particles, mu=1.0): LeafSystem.__init__(self) self._DeclareInputPort(PortDataType.kVectorValued, num_particles, RandomDistribution.kGaussian) self._DeclareContinuousState(num_particles, num_particles, 0) self._DeclareVectorOutputPort(BasicVector(2 * num_particles), self.CopyStateOut) self.num_particles = num_particles self.mu = mu
def __init__(self, plant): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self._G = plant.GetBodyByName("body").body_frame() self._W = plant.world_frame() self.w_G_port = self.DeclareVectorInputPort("omega_WG", BasicVector(3)) self.v_G_port = self.DeclareVectorInputPort("v_WG", BasicVector(3)) self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput) # TODO(russt): Add missing binding #joint_indices = plant.GetJointIndices(self._iiwa) #self.position_start = plant.get_joint(joint_indices[0]).position_start() #self.position_end = plant.get_joint(joint_indices[-1]).position_start() self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start() self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()
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 __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 __init__( self, plant, T_world_objectPickup, T_world_prethrow, T_world_targetRelease, dbg_state_prints=False # turn this to true to get some helfpul dbg prints ): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self.gripper = plant.GetBodyByName("body") self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.qdot_port = self.DeclareVectorInputPort("iiwa_velocity", BasicVector(7)) self.DeclareVectorOutputPort("wsg_position", BasicVector(1), self.CalcGripperTarget) # important poses self.T_world_objectPickup = T_world_objectPickup self.T_world_prethrow = T_world_prethrow self.T_world_targetRelease = T_world_targetRelease # some constants self.GRIPPER_OPEN = np.array([0.5]) self.GRIPPER_CLOSED = np.array([0.0]) # states self.gripper_picked_up_object = False self.reached_prethrow = False self.at_or_passed_release = False # this helps make sure we're in the right state self.is_robot_stationary = False self.pose_translation_history = collections.deque(maxlen=10) # determines gripper control based on above logic self.should_close_gripper = False self.dbg_state_prints = dbg_state_prints
def __init__(self, feedback_rule, control_period=0.0333, print_period=1.0): LeafSystem.__init__(self) self.feedback_rule = feedback_rule self.print_period = print_period self.control_period = control_period self.last_print_time = -print_period self.DeclareInputPort(PortDataType.kVectorValued, 10) self.DeclareDiscreteState(2) self.DeclarePeriodicDiscreteUpdate(period_sec=control_period) self.DeclareVectorOutputPort(BasicVector(2), self.DoCalcVectorOutput)
def __init__(self, hand, x_nom, num_fingers, manipuland_trajectory_callback=None, manipuland_link_name="manipuland_body", finger_link_name="link_3", mu=0.5, n_grasp_search_iters=200, control_period=0.0333, print_period=1.0): LeafSystem.__init__(self) # Copy lots of stuff self.hand = hand self.x_nom = x_nom self.nq = hand.get_num_positions() self.nu = hand.get_num_actuators() self.manipuland_link_name = manipuland_link_name self.manipuland_trajectory_callback = manipuland_trajectory_callback self.n_grasp_search_iters = n_grasp_search_iters self.mu = mu self.num_fingers = num_fingers self.num_finger_links = (self.nq - 3) / num_fingers self.fingertip_position = np.array([1.2, 0., 0.]) self.print_period = print_period self.last_print_time = -print_period self.shut_up = False self.DeclareInputPort( PortDataType.kVectorValued, hand.get_num_positions() + hand.get_num_velocities()) self.DeclareDiscreteState(self.nu) self.DeclarePeriodicDiscreteUpdate(period_sec=control_period) hand_actuators = hand.get_num_actuators() self.finger_link_indices = [] # The hand plant wants every finger input as a # separate vector. Rather than using a mux down the # road, we'll just output in the format it wants. for i in range(num_fingers): self.DeclareVectorOutputPort( BasicVector(hand_actuators / num_fingers), functools.partial(self.DoCalcVectorOutput, i)) self.finger_link_indices.append( self.hand.FindBody("link_3", model_id=i).get_body_index()) # Generate manipuland planar geometry and sample grasps. self.PlanGraspPoints()
def BalancingLQR(robot): # Design an LQR controller for stabilizing the CartPole around the upright. # Returns a (static) AffineSystem that implements the controller (in # the original CartPole coordinates). context = robot.CreateDefaultContext() context.FixInputPort(0, BasicVector([0])) context.get_mutable_continuous_state_vector().SetFromVector(UprightState()) Q = np.diag((10., 10., 1., 1.)) R = [1] return LinearQuadraticRegulator(robot, context, Q, R)
def ports_init(self, context): self.plant_context = self.plant.GetMyMutableContextFromRoot(context) self.plant.SetPositionsAndVelocities(self.plant_context, self.x0) #door_angle = self.plant.GetPositionsFromArray(self.hinge, self.x0[:8]) self.door_index = self.plant.GetJointByName( 'right_door_hinge').position_start() null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) self.plant.GetInputPort("applied_generalized_force").FixValue( self.plant_context, null_force) self.plant.get_actuation_input_port().FixValue( self.plant_context, [0., 0., 0., 0., 0., 0., 0.]) self.W0[self.door_index] = self.x_w_cov
def __init__(self, m1=1., l1=1., m2=2., l2=2., r=1.0, g=10., input_max=10.): #VectorSystem.__init__(self, # 1, # One input (torque at reaction wheel). # 4) # Four outputs (theta, phi, dtheta, dphi) LeafSystem.__init__(self) # One inputs self.DeclareVectorInputPort("u", BasicVector(1)) # Four outputs (full state) self.DeclareVectorOutputPort("y", BasicVector(4), self.CopyStateOut, prerequisites_of_calc=set([ self.all_state_ticket() ])) #self.CopyStateOut, self.DeclareContinuousState( 4) # Four states (theta, phi, dtheta, dphi). self.m1 = float(m1) self.l1 = float(l1) self.m2 = float(m2) self.l2 = float(l2) self.r = float(r) self.g = float(g) self.input_max = float(input_max) # Go ahead and calculate rotational inertias. # Treat the first link as a point mass. self.I1 = self.m1 * self.l1**2 # Treat the second link as a disk. self.I2 = 0.5 * self.m2 * self.r**2
def pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h, method='zero_order_hold'): """ Arguments ---------- plant : RigidBodyPlant RigidBodyPlant of the robot. linearization_points : list of numpy.ndarray Points in the state space where to linearize the dynamics. X : Polyhedron Overall bounds of the state space. U : Polyhedron Overall bounds of the control space. h : float Sampling time for the time discretization of the PWA system. method : string Method used to discretize each piece of the PWA dynamics ('explicit_euler' or 'zero_order_hold'). Returns ---------- PWA : PieceWiseAffineSystem """ # partition of the state space X_partition = constrained_voronoi(linearization_points, X) domains = [Xi.cartesian_product(U) for Xi in X_partition] # create context context = plant.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() input = BasicVector(np.array([0.])) context.FixInputPort(0, input) # affine systems affine_systems = [] for x in linearization_points: state.SetFromVector(x) taylor_approx = FirstOrderTaylorApproximation(plant, context) affine_system = AffineSystem.from_continuous( taylor_approx.A(), taylor_approx.B(), taylor_approx.f0(), h, method ) affine_systems.append(affine_system) return PieceWiseAffineSystem(affine_systems, domains)
def __init__(self, plant, traj_p_G, traj_R_G, traj_wsg_command): LeafSystem.__init__(self) self.plant = plant self.gripper_body = plant.GetBodyByName("body") self.left_finger_joint = plant.GetJointByName( "left_finger_sliding_joint") self.right_finger_joint = plant.GetJointByName( "right_finger_sliding_joint") self.traj_p_G = traj_p_G self.traj_R_G = traj_R_G self.traj_wsg_command = traj_wsg_command self.plant_context = plant.CreateDefaultContext() self.DeclareVectorOutputPort("position", BasicVector(plant.num_positions()), self.CalcPositionOutput)