def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. # (This makes sure relevant event handlers get called.) LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_control_input = discrete_state. \ get_mutable_vector().get_mutable_value() x = self.EvalVectorInput( context, self.robot_state_input_port.get_index()).get_value() q_des = self.EvalVectorInput( context, self.setpoint_input_port.get_index()).get_value() q_full = x[:self.nq] v_full = x[self.nq:] q = q_full[self.controlled_inds] v = v_full[self.controlled_inds] v_des = np.zeros(1) qerr = q_des - q verr = v_des - v Kp = 1000. Kv = 100. new_control_input[:] = np.clip(Kp * qerr + Kv * verr, -self.max_force, self.max_force)
def __init__(self, draw_timestep=0.033333, facecolor=[1, 1, 1], figsize=None, ax=None): LeafSystem.__init__(self) self.set_name('pyplot_visualization') self.timestep = draw_timestep self.DeclarePeriodicPublish(draw_timestep, 0.0) if ax is None: (self.fig, self.ax) = plt.subplots(facecolor=facecolor, figsize=figsize) else: self.ax = ax self.fig = ax.get_figure() self.ax.axis('equal') self.ax.axis('off') self.record = False self.recorded_contexts = [] self.show = (matplotlib.get_backend().lower() != 'template') def on_initialize(context, event): if self.show: self.fig.show() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
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, 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 _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. # (This makes sure relevant event handlers get called.) LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_control_input = discrete_state. \ get_mutable_vector().get_mutable_value() x = self.EvalVectorInput( context, self.robot_state_input_port.get_index()).get_value() x_des = self.EvalVectorInput( context, self.setpoint_input_port.get_index()).get_value() q = x[:self.nq] v = x[self.nq:] q_des = x_des[:self.nq] v_des = x_des[self.nq:] qerr = (q_des[self.controlled_inds] - q[self.controlled_inds]) verr = (v_des[self.controlled_inds] - v[self.controlled_inds]) kinsol = self.rbt.doKinematics(q, v) # Get the full LHS of the manipulator equations # given the current config and desired accelerations vd_des = np.zeros(self.rbt.get_num_positions()) vd_des[self.controlled_inds] = 1000. * qerr + 100 * verr lhs = self.rbt.inverseDynamics(kinsol, external_wrenches={}, vd=vd_des) new_u = self.B_inv.dot(lhs[self.controlled_inds]) new_control_input[:] = new_u
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, draw_timestep=0.033333, facecolor=[1, 1, 1], figsize=None, ax=None): LeafSystem.__init__(self) self.set_name('pyplot_visualization') self.timestep = draw_timestep self.DeclarePeriodicPublish(draw_timestep, 0.0) if ax is None: (self.fig, self.ax) = plt.subplots(facecolor=facecolor, figsize=figsize) else: self.ax = ax self.fig = ax.get_figure() self.ax.axis('equal') self.ax.axis('off') self.record = False self.recorded_contexts = [] self.show = (matplotlib.get_backend().lower() != 'template') def on_initialize(context, event): if self.show: self.fig.show() self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
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, rbtree, old_mrbv=None, draw_timestep=0.033333, prefix="RBViz", zmq_url="tcp://127.0.0.1:6000", draw_collision=False, clear_vis=False): LeafSystem.__init__(self) self.set_name('meshcat_visualization') self.timestep = draw_timestep self._DeclarePeriodicPublish(draw_timestep, 0.0) self.rbtree = rbtree self.draw_collision = draw_collision self._DeclareInputPort( PortDataType.kVectorValued, self.rbtree.get_num_positions() + self.rbtree.get_num_velocities()) # Set up meshcat self.prefix = prefix self.vis = meshcat.Visualizer(zmq_url=zmq_url) self.old_mrbv = old_mrbv # Don't both with caching logic if prefixes are different if self.old_mrbv is not None and self.old_mrbv.prefix != self.prefix: raise ValueError("Can't do any caching since old_mrbv has ", "different prefix than current mrbv.") self.body_names = [] if clear_vis: self.vis.delete() # Publish the tree geometry to get the visualizer started self.PublishAllGeometry()
def __init__(self, camera, rbt, draw_timestep=0.033333, prefix="RBCameraViz", zmq_url="tcp://127.0.0.1:6000"): LeafSystem.__init__(self) self.set_name('camera meshcat visualization') self.timestep = draw_timestep self._DeclarePeriodicPublish(draw_timestep, 0.0) self.camera = camera self.rbt = rbt self.prefix = prefix self.camera_input_port = \ self._DeclareAbstractInputPort( "depth_im", AbstractValue.Make(Image[PixelType.kDepth32F]( self.camera.depth_camera_info().width(), self.camera.depth_camera_info().height()))) self.state_input_port = \ self._DeclareInputPort(PortDataType.kVectorValued, rbt.get_num_positions() + rbt.get_num_velocities()) # Set up meshcat self.vis = meshcat.Visualizer(zmq_url=zmq_url) self.vis[prefix].delete()
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 _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. # (This makes sure relevant event handlers get called.) LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_control_input = discrete_state. \ get_mutable_vector().get_mutable_value() x = self.EvalVectorInput( context, self.robot_state_input_port.get_index()).get_value() gripper_width_des = self.EvalVectorInput( context, self.setpoint_input_port.get_index()).get_value() q_full = x[:self.nq] v_full = x[self.nq:] tree = self.plant.tree() q_hand = tree.get_positions_from_array(self.model_instance, q_full) q_hand_des = np.array([-gripper_width_des[0], gripper_width_des[0]]) v_hand = tree.get_velocities_from_array(self.model_instance, v_full) v_hand_des = np.zeros(2) qerr_hand = q_hand_des - q_hand verr_hand = v_hand_des - v_hand Kp = 1000. Kv = 100. new_control_input[:] = np.clip(Kp * qerr_hand + Kv * verr_hand, -self.max_force, self.max_force)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_state = discrete_state.get_mutable_vector().get_mutable_value() # Close gripper after plan has been executed new_state[:] = self.gripper_setpoint_list[self.current_plan_idx]
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): LeafSystem.__init__(self) self.DeclareContinuousState(1, 1, 0) self.mu = 0.2 self.last_poincare = None # placeholder for writing return map result. self.poincare_witness = self.MakeWitnessFunction( "poincare", WitnessFunctionDirection.kNegativeThenNonNegative, self.poincare, UnrestrictedUpdateEvent(self.record_poincare))
def __init__(self, body_index): LeafSystem.__init__(self) self.body_index = body_index self.DeclareAbstractInputPort( "poses", plant.get_body_poses_output_port().Allocate()) self.DeclareAbstractOutputPort( "pose", lambda: AbstractValue.Make(RigidTransform()), self.CalcOutput)
def __init__(self, num_particles, mu=1.0): LeafSystem.__init__(self) self.DeclareInputPort("noise", 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.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem._DoCalcDiscreteVariableUpdates( self, context, events, discrete_state) new_control_input = discrete_state. \ get_mutable_vector().get_mutable_value() x = self.EvalVectorInput(context, 0).get_value() old_u = discrete_state.get_mutable_vector().get_mutable_value() new_u = self.ComputeControlInput(x, context.get_time()) new_control_input[:] = new_u
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_state = discrete_state. \ get_mutable_vector().get_mutable_value() # Close gripper after plan has been executed if context.get_time() > self.qtraj.end_time(): new_state[:] = 0. else: new_state[:] = 0.1
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, 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, 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 __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 __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, draw_timestep=0.033333, facecolor=[1, 1, 1], figsize=None): LeafSystem.__init__(self) self.set_name('pyplot_visualization') self.timestep = draw_timestep self._DeclarePeriodicPublish(draw_timestep, 0.0) (self.fig, self.ax) = plt.subplots(facecolor=facecolor, figsize=figsize) self.ax.axis('equal') self.ax.axis('off') self.fig.show()
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, 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)
def __init__(self, rbtree, draw_timestep=0.033333, prefix="RBViz", zmq_url="tcp://127.0.0.1:6000", draw_collision=False): LeafSystem.__init__(self) self.set_name('meshcat_visualization') self.timestep = draw_timestep self.DeclarePeriodicPublish(draw_timestep, 0.0) self.rbtree = rbtree self.draw_collision = draw_collision self.DeclareInputPort(PortDataType.kVectorValued, self.rbtree.get_num_positions() + self.rbtree.get_num_velocities()) # Set up meshcat self.prefix = prefix self.vis = meshcat.Visualizer(zmq_url=zmq_url) self.vis[self.prefix].delete() # Publish the tree geometry to get the visualizer started self.PublishAllGeometry()