def __init__(self, robot, frame_E, parameters, time_step): """ @param robot is a reference to a MultibodyPlant. @param frame_E is a multibody::Frame on the robot. @param params is a DifferentialIKParams. @params time_step This system updates its state/outputs at discrete periodic intervals defined with period @p time_step. """ LeafSystem.__init__(self) self.robot = robot self.frame_E = frame_E self.parameters = parameters self.parameters.set_timestep(time_step) self.time_step = time_step # Note that this context is NOT the context of the DifferentialIK # system, but rather a context for the multibody plant that is used # to pass the configuration into the DifferentialInverseKinematics # methods. self.robot_context = robot.CreateDefaultContext() # Confirm that all velocities are zero (they will not be reset below). assert not self.robot.GetPositionsAndVelocities( self.robot_context)[-robot.num_velocities():].any() # Store the robot positions as state. self.DeclareDiscreteState(robot.num_positions()) self.DeclarePeriodicDiscreteUpdate(time_step) # Desired pose of frame E in world frame. self.DeclareInputPort("rpy_xyz_desired", PortDataType.kVectorValued, 6) # Provide the output as desired positions. self.DeclareVectorOutputPort("joint_position_desired", BasicVector( robot.num_positions()), self.CopyPositionOut)
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())
def __init__(self, frame_id): LeafSystem.__init__(self) self.DeclareAbstractOutputPort( "goal", lambda: AbstractValue.Make(FramePoseVector()), self.OutputGoal) self.goal = RigidTransform(np.zeros(3)) self.frame_id = frame_id
def __init__(self): LeafSystem.__init__(self) self.set_name('pendulum_visualizer') self._DeclareInputPort(PortDataType.kVectorValued, 2) self._DeclarePeriodicPublish(0.03, 0.0) (self.fig, ax) = plt.subplots() ax.axis('equal') ax.axis('off') ax.set_xlim([-1.2, 1.2]) ax.set_ylim([-1.2, 1.2]) self.base = ax.fill(self.base_x, self.base_y, zorder=1, color=(.3, .6, .4), edgecolor='k') # arm_x and arm_y are closed (last element == first element), but don't pass the # last element to the fill command, because it gets closed anyhow (and we want the # sizes to match for the update). self.arm = ax.fill(self.arm_x[0:-1], self.arm_y[0:-1], zorder=0, color=(.9, .1, 0), edgecolor='k') self.center_of_mass = ax.plot(0, -self.ac1, zorder=1, color='b', marker='o', markersize=14) self.draw(0) self.fig.show() self.ax = ax
def __init__(self, robot, frame_E, parameters, time_step): """ @param robot is a reference to a MultibodyPlant. @param frame_E is a multibody::Frame on the robot. @param params is a DifferentialIKParams. @params time_step This system updates its state/outputs at discrete periodic intervals defined with period @p time_step. """ LeafSystem.__init__(self) self.robot = robot self.frame_E = frame_E self.parameters = parameters self.parameters.set_timestep(time_step) self.time_step = time_step # Note that this context is NOT the context of the DifferentialIK # system, but rather a context for the multibody plant that is used # to pass the configuation into the DifferentialInverseKinematics # methods. self.robot_context = robot.CreateDefaultContext() # Confirm that all velocities are zero (they will not be reset below). assert not self.robot.tree().GetPositionsAndVelocities( self.robot_context)[-robot.num_velocities():].any() # Store the robot positions as state. self._DeclareDiscreteState(robot.num_positions()) self._DeclarePeriodicDiscreteUpdate(time_step) # Desired pose of frame E in world frame. self._DeclareInputPort("rpy_xyz_desired", PortDataType.kVectorValued, 6) # Provide the output as desired positions. self._DeclareVectorOutputPort("joint_position_desired", BasicVector(robot.num_positions()), self.CopyPositionOut)
def __init__(self, *args, update_period_sec=0.1): """ Constructs the system and displays the widgets. Args: update_period_sec: Specifies how often the kernel is queried for widget ui events. An output port is created for each element in *args. Each element of args must itself be an iterable collection (list, tuple, set, ...) of ipywidget elements, whose values will be concatenated into a single vector output. """ LeafSystem.__init__(self) self._widgets = args for i, widget_iterable in enumerate(self._widgets): for w in widget_iterable: assert isinstance( w, Widget), ("args must be collections of widgets") display(w) port = self.DeclareVectorOutputPort( f"widget_group_{i}", BasicVector(len(widget_iterable)), partial(self.DoCalcOutput, port_index=i)) port.disable_caching_by_default() self.DeclarePeriodicPublish(update_period_sec, 0.0)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) self._GetCurrentPlan(context) t= context.get_time() q_iiwa = self.EvalVectorInput( context, self.iiwa_position_input_port.get_index()).get_value() v_iiwa = self.EvalVectorInput( context, self.iiwa_velocity_input_port.get_index()).get_value() tau_iiwa = self.EvalVectorInput( context, self.iiwa_external_torque_input_port.get_index()).get_value() t_plan = t - self.current_plan_start_time new_control_output = discrete_state.get_mutable_vector().get_mutable_value() new_control_output[0:self.nu] = \ self.current_plan.CalcPositionCommand(q_iiwa, v_iiwa, tau_iiwa, t_plan, self.control_period) new_control_output[self.nu:2*self.nu] = \ self.current_plan.CalcTorqueCommand(q_iiwa, v_iiwa, tau_iiwa, t_plan, self.control_period) new_control_output[14] = self.current_gripper_setpoint # print current simulation time if (self.print_period and t - self.last_print_time >= self.print_period): print "t: ", t self.last_print_time = t
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 self.called_per_step = False self.called_periodic = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate(period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent(trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent(trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(PortDataType.kVectorValued, 1) self.DeclareVectorInputPort(name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self, draw_period=1. / 30, facecolor=[1, 1, 1], figsize=None, ax=None): LeafSystem.__init__(self) self.set_name('pyplot_visualization') self.timestep = draw_period self.DeclarePeriodicPublish(draw_period, 0.0) if ax is None: self.fig = plt.figure(facecolor=facecolor, figsize=figsize) self.ax = plt.axes() self.fig.add_axes(self.ax) else: self.ax = ax self.fig = ax.get_figure() self.ax.axis('equal') self.ax.axis('off') 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, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=RigidTransform.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = RigidTransform(X_WP) self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer_' + name) self.DeclarePeriodicPublish(draw_period, 0.0) self.DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud()))
def __init__(self, visualization_topic="/drake", period_sec=1. / 60, tf_topic="/tf", role=Role.kPerception): """ Arguments: ... role: The Role to visualize geometry for. """ LeafSystem.__init__(self) self._role = role self._geometry_query = self.DeclareAbstractInputPort( "geometry_query", AbstractValue.Make(QueryObject())) self.DeclareInitializationEvent( event=PublishEvent(trigger_type=TriggerType.kInitialization, callback=self._initialize)) self.DeclarePeriodicEvent(period_sec=period_sec, offset_sec=0., event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._publish_tf)) # TODO(eric.cousineau): Rather than explicitly allocate publishers, # this should probably instead output the messages. Either together as a # tuple, or separately. Should delegate that to `ConnectRvizVisualizer`. self._marker_pub = rospy.Publisher(visualization_topic, MarkerArray, queue_size=1) self._tf_pub = rospy.Publisher(tf_topic, TFMessage, queue_size=1) self._marker_array_old = None self._marker_array_old_stamp = None
def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("touchdown_angle", BasicVector(1)) self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0) self.DeclareVectorOutputPort("state", BasicVector(8), self.CopyStateOut) # Parameters from Geyer05, p.23 self.mass = 80. # kg self.r0 = 1. # m self.gravity = 9.81 # m/s^2 # Define spring constant in terms of the dimensionless number. # Definition in section 2.4.3, values in figure 2.4. # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts). dimensionless_spring_constant = 10.7 self.stiffness = (dimensionless_spring_constant * self.mass * self.gravity / self.r0) self.last_apex = None # placeholder for writing return map result. self.touchdown_witness = self.MakeWitnessFunction( "touchdown", WitnessFunctionDirection.kPositiveThenNonPositive, self.foot_height, UnrestrictedUpdateEvent(self.touchdown)) self.takeoff_witness = self.MakeWitnessFunction( "takeoff", WitnessFunctionDirection.kPositiveThenNonPositive, self.leg_compression, UnrestrictedUpdateEvent(self.takeoff)) self.apex_witness = self.MakeWitnessFunction( "apex", WitnessFunctionDirection.kPositiveThenNonPositive, self.apex, PublishEvent(self.publish_apex))
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, meshcat, planar=False): """ @param meshcat The already created pydrake.geometry.Meshcat instance. @param planar if True, the GUI will not have Pitch, Yaw, or Y-axis sliders and default values will be returned. """ LeafSystem.__init__(self) self.DeclareVectorOutputPort("rpy_xyz", 6, self.DoCalcOutput) self.meshcat = meshcat self.planar = planar # Rotation control sliders. self.meshcat.AddSlider( name=self._ROLL.name, min=-2.0 * np.pi, max=2.0 * np.pi, step=0.01, value=self._ROLL.default) if not self.planar: self.meshcat.AddSlider( name=self._PITCH.name, min=-2.0 * np.pi, max=2.0 * np.pi, step=0.01, value=self._PITCH.default) self.meshcat.AddSlider( name=self._YAW.name, min=-2.0 * np.pi, max=2.0 * np.pi, step=0.01, value=self._YAW.default) # Position control sliders. self.meshcat.AddSlider( name=self._X.name, min=-0.6, max=0.8, step=0.01, value=self._X.default) if not self.planar: self.meshcat.AddSlider( name=self._Y.name, min=-0.8, max=0.3, step=0.01, value=self._Y.default) self.meshcat.AddSlider( name=self._Z.name, min=0.0, max=1.1, step=0.01, value=self._Z.default)
def __init__(self, scene_graph, draw_period=0.033333, prefix="drake", zmq_url="tcp://127.0.0.1:6000"): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a non-default url to connect to the visualizer. The default value is the url obtained by running `meshcat-server` in another terminal. If zmp_url=None, then then a new server will be automatically started (as a child of this process). """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self._DeclarePeriodicPublish(draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self._DeclareInputPort("lcm_visualization", PortDataType.kAbstractValued, 0) # Set up meshcat. self.prefix = prefix print("Connecting to meshcat-server...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) print("Connected.") self.vis[self.prefix].delete() self._scene_graph = scene_graph
def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("touchdown_angle", BasicVector(1)) self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0) self.DeclareVectorOutputPort("state", BasicVector(8), self.CopyStateOut) # Parameters from Geyer05, p.23 self.mass = 80. # kg self.r0 = 1. # m self.gravity = 9.81 # m/s^2 # Define spring constant in terms of the dimensionless number. # Definition in section 2.4.3, values in figure 2.4. # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts). dimensionless_spring_constant = 10.7 self.stiffness = \ dimensionless_spring_constant*self.mass*self.gravity/self.r0 self.last_apex = None # placeholder for writing return map result. self.touchdown_witness = self.MakeWitnessFunction( "touchdown", WitnessFunctionDirection.kPositiveThenNonPositive, self.foot_height, UnrestrictedUpdateEvent(self.touchdown)) self.takeoff_witness = self.MakeWitnessFunction( "takeoff", WitnessFunctionDirection.kPositiveThenNonPositive, self.leg_compression, UnrestrictedUpdateEvent(self.takeoff)) self.apex_witness = self.MakeWitnessFunction( "apex", WitnessFunctionDirection.kPositiveThenNonPositive, self.apex, PublishEvent(self.publish_apex))
def __init__(self, funcs, scalar_as_vector=True, set_name=True): """ Arguments: func: Function with necessary annotations. If the output is not annotated, then the function will be called with sample arguments. This may cause unwanted side-effects, especially when printing, so you should consider annotating the output type. scalar_as_vector: If True, inputs and outputs that are of (float, AutoDiffXd, Expression) will interpreted as VectorArg(1). """ LeafSystem.__init__(self) if not isinstance(funcs, (list, tuple)): funcs = (funcs, ) assert len(funcs) > 0 cls = type(self) if set_name: func_names = ", ".join([f"{func.__name__}" for func in funcs]) self.set_name(f"{cls.__name__}[{func_names}]") # TODO(eric.cousineau): Support sharing inputs with multiple functions. self._func_declaration = [ _FunctionDeclaration(self, func, scalar_as_vector=scalar_as_vector) for func in funcs ]
def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=Isometry3.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = X_WP self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer') self._DeclarePeriodicPublish(draw_period, 0.0) self._DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud()))
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): LeafSystem.__init__(self) self._DeclareAbstractOutputPort( "X_WE", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self._DeclarePeriodicPublish(0.1, 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.1, to=0.8, resolution=-1, label="x", length=800, orient=tk.HORIZONTAL) self.x.pack() self.y = tk.Scale(self.window, from_=-0.3, to=0.3, resolution=-1, label="y", length=800, orient=tk.HORIZONTAL) self.y.pack() self.z = tk.Scale(self.window, from_=0, to=0.8, resolution=-1, label="z", length=800, orient=tk.HORIZONTAL) self.z.pack()
def __init__(self, config_file, viz=False, segment_scene_function=None, get_pose_function=None): """ A system that takes in 3 Drake PointClouds and ImageRgba8U from RGBDCameras and determines the pose of an object in them. The user must supply a segmentation function and pose alignment to determine the pose. If these functions aren't supplied, the returned pose will always be the 4x4 identity matrix. @param config_file str. A path to a .yml configuration file for the cameras. @param viz bool. If True, save the aligned and segmented point clouds as serialized numpy arrays. @param segment_scene_function A Python function that returns a subset of the scene point cloud. See self.SegmentScene for more details. @param get_pose_function A Python function that calculates a pose from a segmented point cloud. See self.GetPose for more details. @system{ @input_port{camera_left_rgb}, @input_port{camera_middle_rgb}, @input_port{camera_right_rgb}, @input_port{left_point_cloud}, @input_port{middle_point_cloud}, @input_port{right_point_cloud}, @output_port{X_WObject} } """ LeafSystem.__init__(self) # TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB # fields. self.left_rgb = self._DeclareAbstractInputPort( "camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.middle_rgb = self._DeclareAbstractInputPort( "camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.right_rgb = self._DeclareAbstractInputPort( "camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.left_depth = self._DeclareAbstractInputPort( "left_point_cloud", AbstractValue.Make(mut.PointCloud())) self.middle_depth = self._DeclareAbstractInputPort( "middle_point_cloud", AbstractValue.Make(mut.PointCloud())) self.right_depth = self._DeclareAbstractInputPort( "right_point_cloud", AbstractValue.Make(mut.PointCloud())) self._DeclareAbstractOutputPort( "X_WObject", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self.segment_scene_function = segment_scene_function self.get_pose_function = get_pose_function self._LoadConfigFile(config_file) self.viz = viz
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() new_u = self.ComputeControlInput(x, context.get_time()) new_control_input[:] = new_u
def __init__(self, meshcat): LeafSystem.__init__(self) port = self.DeclareVectorOutputPort("wsg_position", 1, self.DoCalcOutput) port.disable_caching_by_default() self._meshcat = meshcat self._button = "Open/Close Gripper" meshcat.AddButton(self._button)
def __init__(self): LeafSystem.__init__(self) self.num_vec = 3 # self._DeclareContinuousState(num_joints, num_particles, 0) self._DeclareVectorOutputPort(BasicVector(self.num_vec), self.CalcOutput)
def __init__(self): LeafSystem.__init__(self) self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6), self._DoCalcOutput) self._DeclarePeriodicPublish(0.1, 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.1, to=0.8, resolution=-1, label="x", length=800, orient=tk.HORIZONTAL) self.x.pack() self.y = tk.Scale(self.window, from_=-0.3, to=0.3, resolution=-1, label="y", length=800, orient=tk.HORIZONTAL) self.y.pack() self.z = tk.Scale(self.window, from_=0, to=0.8, resolution=-1, label="z", length=800, orient=tk.HORIZONTAL) self.z.pack()
def __init__(self): LeafSystem.__init__(self) # A 1D input vector for acceleration. self.DeclareInputPort('acceleration', PortDataType.kVectorValued, 1) # Adding one generalized position and one generalized velocity. self.DeclareContinuousState(1, 1, 0) # A 2D output vector for position and velocity. self.DeclareVectorOutputPort('postion_and_velocity', BasicVector(2), self.CopyStateOut)
def __init__(self): LeafSystem.__init__(self) # Output Port self.lcm_message_port_index = self._DeclareAbstractOutputPort( 'state_output_port', self._Allocator, self._OutputRobotState, ).get_index()
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_continuous = False self.called_discrete = False self.called_initialize = False self.called_per_step = False self.called_periodic = False self.called_getwitness = False self.called_witness = False self.called_guard = False self.called_reset = False self.called_system_reset = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate(period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent(trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent(trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(kUseDefaultName, PortDataType.kVectorValued, 1) self.DeclareVectorInputPort(name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort("noop", BasicVector(1), noop, prerequisites_of_calc=set( [self.nothing_ticket()])) self.witness = self.MakeWitnessFunction( "witness", WitnessFunctionDirection.kCrossesZero, self._witness) # Test bindings for both callback function signatures. self.reset_witness = self.MakeWitnessFunction( "reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent(self._reset)) self.system_reset_witness = self.MakeWitnessFunction( "system reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent( system_callback=self._system_reset))
def __init__(self, plant): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._panda = plant.GetModelInstanceByName("panda") self._G = plant.GetBodyByName("panda_hand").body_frame() self._W = plant.world_frame() self.DeclareVectorInputPort("panda_position", BasicVector(9)) self.DeclareVectorOutputPort("panda_velocity", BasicVector(9), self.CalcOutput)
def __init__(self, actuators): LeafSystem.__init__(self) self.num_actuators = len(actuators) self.actuators_init = np.ones(self.num_actuators) * 0.0 self.robot_command_port_index = self._DeclareAbstractInputPort( 'robot_command_port', AbstractValue.Make(littledog_command_t)).get_index() self.desired_effort_port_indices = self._DeclareVectorOutputPort( BasicVector(self.num_actuators), self.OutputActuation).get_index()
def __init__(self): LeafSystem.__init__(self) # parameters self.m = 1.0 # definitions self.DeclareContinuousState(2) # z=[x, xdot] self.DeclareVectorInputPort("u", BasicVector(1)) # force self.DeclareVectorOutputPort("x", BasicVector(1), self.CopyStateOut, \ prerequisites_of_calc=set([self.all_state_ticket()])) # outputs - x
def _DoPublish(self, context, events): # Call base method to ensure we do not get recursion. LeafSystem._DoPublish(self, context, events) # N.B. We do not test for a singular call to `DoPublish` # (checking `assertFalse(self.called_publish)` first) because # the above `_DeclareInitializationEvent` will call both its # callback and this event when invoked via # `Simulator::Initialize` from `call_leaf_system_overrides`, # even when we explicitly say not to publish at initialize. self.called_publish = True
def __init__(self, plant, traj_hand): LeafSystem.__init__(self) self.plant = plant self.gripper_body = plant.GetBodyByName("panda_hand") self.left_finger_joint = plant.GetJointByName("panda_finger_joint1") self.right_finger_joint = plant.GetJointByName("panda_finger_joint2") self.traj_hand = traj_hand self.plant_context = plant.CreateDefaultContext() self.DeclareVectorOutputPort("finger_position", BasicVector(2), self.CalcPositionOutput)
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 self.called_per_step = False self.called_periodic = False self.called_getwitness = False self.called_witness = False self.called_guard = False self.called_reset = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate( period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent( trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(PortDataType.kVectorValued, 1) self.DeclareVectorInputPort( name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort( "noop", BasicVector(1), noop, prerequisites_of_calc=set([self.nothing_ticket()])) self.witness = self.MakeWitnessFunction( "witness", WitnessFunctionDirection.kCrossesZero, self._witness) self.reset_witness = self.MakeWitnessFunction( "reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent(self._reset))
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_discrete = 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._DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareVectorOutputPort(BasicVector(1), noop)
def test_deprecated_protected_aliases(self): """Tests a subset of protected aliases, pursuant to #9651.""" class OldSystem(LeafSystem): 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 _DoPublish(self, context, events): self.called_publish = True # Ensure old overrides are still used system = OldSystem() context = system.CreateDefaultContext() with catch_drake_warnings(expected_count=1): system.Publish(context) self.assertTrue(system.called_publish) # Ensure documentation doesn't duplicate stuff. with catch_drake_warnings(expected_count=1): self.assertIn("deprecated", LeafSystem._DoPublish.__doc__) # This will warn both on (a) calling the method and (b) on the # invocation of the override. with catch_drake_warnings(expected_count=2): LeafSystem._DoPublish(system, context, []) class AccidentallyBothSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.called_old_publish = False self.called_new_publish = False def DoPublish(self, context, events): self.called_new_publish = True def _DoPublish(self, context, events): self.called_old_publish = True system = AccidentallyBothSystem() context = system.CreateDefaultContext() # This will trigger no deprecations, as the newer publish is called. system.Publish(context) self.assertTrue(system.called_new_publish) self.assertFalse(system.called_old_publish)
def __init__(self, index): LeafSystem.__init__(self) num_q = 2 num_v = 1 num_z = 3 num_state = num_q + num_v + num_z if index == 0: self._DeclareContinuousState(num_state_variables=num_state) elif index == 1: self._DeclareContinuousState( num_q=num_q, num_v=num_v, num_z=num_z) elif index == 2: self._DeclareContinuousState(BasicVector(num_state)) elif index == 3: self._DeclareContinuousState( BasicVector(num_state), num_q=num_q, num_v=num_v, num_z=num_z)
def _DoPublish(self, context, event): # TODO(russt): Change this to declare a periodic event with a # callback instead of overriding _DoPublish, pending #9992. LeafSystem._DoPublish(self, context, event) pose_bundle = self.EvalAbstractInput(context, 0).get_value() for frame_i in range(pose_bundle.get_num_poses()): # SceneGraph currently sets the name in PoseBundle as # "get_source_name::frame_name". [source_name, frame_name] = pose_bundle.get_name(frame_i)\ .split("::") model_id = pose_bundle.get_model_instance_id(frame_i) # The MBP parsers only register the plant as a nameless source. # TODO(russt): Use a more textual naming convention here? self.vis[self.prefix][source_name][str(model_id)][frame_name]\ .set_transform(pose_bundle.get_pose(frame_i).matrix())
def __init__(self, window=None, open_position=0.107, closed_position=0.002, force_limit=40, update_period_sec=0.05): """" Args: window: Optionally pass in a tkinter.Tk() object to add these widgets to. Default behavior is to create a new window. update_period_sec: Specifies how often the window update() method gets called. open_position: Target position for the finger when open. closed_position: Target position for the gripper when closed. force_limit: Force limit to send to Schunk WSG controller. """ LeafSystem.__init__(self) self._DeclareVectorOutputPort("position", BasicVector(1), self.CalcPositionOutput) self._DeclareVectorOutputPort("force_limit", BasicVector(1), self.CalcForceLimitOutput) if window is None: self.window = tk.Tk() self.window.title(title) else: self.window = window # Schedule window updates in either case (new or existing window): self._DeclarePeriodicPublish(update_period_sec, 0.0) self._open_button = tk.Button(self.window, text="Open Gripper", state=tk.DISABLED, command=self.open) self._open_button.pack() self._close_button = tk.Button(self.window, text="Close Gripper", command=self.close) self._close_button.pack() self._open_state = True self._open_position = open_position self._closed_position = closed_position self._force_limit = force_limit self.window.bind("<space>", self._space_callback)
def __init__(self, grab_focus=True): LeafSystem.__init__(self) self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6), self._DoCalcOutput) self._DeclareVectorOutputPort("position", BasicVector(1), self.CalcPositionOutput) self._DeclareVectorOutputPort("force_limit", BasicVector(1), self.CalcForceLimitOutput) # 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.teleop_manager = TeleopMouseKeyboardManager(grab_focus=grab_focus) self.roll = self.pitch = self.yaw = 0 self.x = self.y = self.z = 0 self.gripper_max = 0.107 self.gripper_min = 0.01 self.gripper_goal = self.gripper_max
def __init__(self, meshcat_viz, force_threshold=1e-2, contact_force_scale=10, plant=None): """ Args: meshcat_viz: a MeshcatVisualizer object. force_threshold: contact forces whose norms are smaller than force_threshold are not displayed. contact_force_scale: a contact force with norm F (in Newtons) is displayed as a cylinder with length F/contact_force_scale (in meters). plant: the MultibodyPlant associated with meshcat_viz.scene_graph. """ LeafSystem.__init__(self) assert plant is not None self._meshcat_viz = meshcat_viz self._force_threshold = force_threshold self._contact_force_scale = contact_force_scale self._plant = plant self.set_name('meshcat_contact_visualizer') self._DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self._DeclareAbstractInputPort("pose_bundle", AbstractValue.Make(PoseBundle(0))) # Contact results input port from MultibodyPlant self._DeclareAbstractInputPort( "contact_results", AbstractValue.Make(ContactResults())) # Make force cylinders smaller at initialization. self._force_cylinder_radial_scale = 1. self._force_cylinder_longitudinal_scale = 100. # This system has undeclared states, see #4330. # - All contacts (previous and current), of type `_ContactState`. self._contacts = [] # - Unique key for contacts in meshcat. self._contact_key_counter = 0 # - Previous time at which contact was published. self._t_previous = 0.
def _DoHasDirectFeedthrough(self, input_port, output_port): # Test inputs. test.assertEqual(input_port, 0) test.assertEqual(output_port, 0) # Call base method to ensure we do not get recursion. base_return = LeafSystem._DoHasDirectFeedthrough( self, input_port, output_port) test.assertTrue(base_return is None) # Return custom methods. self.called_feedthrough = True return False
def _DoPublish(self, context, event): LeafSystem._DoPublish(self, context, event) point_cloud_P = self.EvalAbstractInput(context, 0).get_value() # `Q` is a point in the point cloud. p_PQs = point_cloud_P.xyzs() # Use only valid points. valid = np.logical_not(np.isnan(p_PQs)) valid = np.all(valid, axis=0) # Reduce along XYZ axis. p_PQs = p_PQs[:, valid] if point_cloud_P.has_rgbs(): rgbs = point_cloud_P.rgbs()[:, valid] else: # Need manual broadcasting. count = p_PQs.shape[1] rgbs = np.tile(np.array([self._default_rgb]).T, (1, count)) # pydrake `PointCloud.rgbs()` are on [0..255], while meshcat # `PointCloud` colors are on [0..1]. rgbs = rgbs / 255. # Do not use in-place so we can promote types. # Send to meshcat. self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs)) self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())
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 self.called_per_step = False self.called_periodic = False # Ensure we have desired overloads. self._DeclarePeriodicPublish(1.0) self._DeclarePeriodicPublish(1.0, 0) self._DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self._DeclarePeriodicDiscreteUpdate( period_sec=1.0, offset_sec=0.) self._DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self._DeclarePerStepEvent( event=PublishEvent( trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self._DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self._DeclareContinuousState(2) self._DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareVectorInputPort( name="test_input", model_vector=BasicVector(1), random_type=None) self._DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self): LeafSystem.__init__(self) self._DeclareContinuousState(1) self._DeclareDiscreteState(2)
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 __init__(self): LeafSystem.__init__(self) self._DeclareContinuousState(1) self._DeclareDiscreteState(2) self._DeclareAbstractState(model_value.Clone())
def __init__(self): LeafSystem.__init__(self) self.DeclareAbstractInputPort( "header_t", AbstractValue.Make(header_t)) self.DeclareVectorOutputPort( BasicVector(1), self._calc_output)
def __init__(self): LeafSystem.__init__(self) self.called_old_publish = False self.called_new_publish = False
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 _DoPublish(self, context, events): # Call base method to ensure we do not get recursion. LeafSystem._DoPublish(self, context, events) self.called_publish = True
def __init__(self, scene_graph, draw_period=0.033333, prefix="drake", zmq_url="default", open_browser=None): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a url to connect to the visualizer. Use zmp_url="default" to the value obtained by running a single `meshcat-server` in another terminal. Use zmp_url=None or zmq_url="new" to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. zmq_url="tcp://127.0.0.1:6000" to specify a specific address. open_browser: Set to True to open the meshcat browser url in your default web browser. The default value of None will open the browser iff a new meshcat server is created as a subprocess. Set to False to disable this. Note: This call will not return until it connects to the meshcat-server. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self._DeclarePeriodicPublish(draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self._DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" elif zmq_url == "new": zmq_url = None if zmq_url is None and open_browser is None: open_browser = True # Set up meshcat. self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self._DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def _DoCalcDiscreteVariableUpdates( self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem._DoCalcDiscreteVariableUpdates( self, context, events, discrete_state) self.called_discrete = True
def _DoPublish(self, context, event): LeafSystem._DoPublish(self, context, event) pose_bundle = self.EvalAbstractInput(context, 0).get_value() X_WB_map = self._get_pose_map(pose_bundle) self._draw_contact_forces(context, X_WB_map)
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, 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)
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))