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):
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, 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 _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):
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))
