def plant_system(self):
'''
Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant
'''
# Add Systems
builder = DiagramBuilder()
self.scene_graph = builder.AddSystem(SceneGraph())
self.mbp = builder.AddSystem(MultibodyPlant())
# Load the model from the file
AddModelFromSdfFile(file_name=self.model_path,
plant=self.mbp,
scene_graph=self.scene_graph)
self.mbp.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
self.mbp.Finalize(self.scene_graph)
assert self.mbp.geometry_source_is_registered()
# Visualizer must be initialized after Finalize() and before CreateDefaultContext()
self.init_visualizer()
builder.Connect(
self.mbp.get_geometry_poses_output_port(),
self.scene_graph.get_source_pose_port(self.mbp.get_source_id()))
# Expose the inputs and outputs and build the diagram
self._input_port_index_action = builder.ExportInput(
self.mbp.get_actuation_input_port())
self._output_port_index_state = builder.ExportOutput(
self.mbp.get_continuous_state_output_port())
self.diagram = builder.Build()
self._output = self.mbp.AllocateOutput()
return self.diagram
def PendulumExample():
builder = DiagramBuilder()
plant = builder.AddSystem(PendulumPlant())
scene_graph = builder.AddSystem(SceneGraph())
PendulumGeometry.AddToBuilder(builder, plant.get_state_output_port(),
scene_graph)
MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(publish_period=np.inf))
builder.ExportInput(plant.get_input_port(), "torque")
# Note: The Pendulum-v0 in gym outputs [cos(theta), sin(theta), thetadot]
builder.ExportOutput(plant.get_state_output_port(), "state")
# Add a camera (I have sugar for this in the manip repo)
X_WC = RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2),
[0, -1.5, 0])
rgbd = AddRgbdSensor(builder, scene_graph, X_WC)
builder.ExportOutput(rgbd.color_image_output_port(), "camera")
diagram = builder.Build()
simulator = Simulator(diagram)
def reward(system, context):
plant_context = plant.GetMyContextFromRoot(context)
state = plant_context.get_continuous_state_vector()
u = plant.get_input_port().Eval(plant_context)[0]
theta = state[0] % 2 * np.pi # Wrap to 2*pi
theta_dot = state[1]
return (theta - np.pi)**2 + 0.1 * theta_dot**2 + 0.001 * u
max_torque = 3
env = DrakeGymEnv(simulator,
time_step=0.05,
action_space=gym.spaces.Box(low=np.array([-max_torque]),
high=np.array([max_torque])),
observation_space=gym.spaces.Box(
low=np.array([-np.inf, -np.inf]),
high=np.array([np.inf, np.inf])),
reward=reward,
render_rgb_port_id="camera")
check_env(env)
if show:
env.reset()
image = env.render(mode='rgb_array')
fig, ax = plt.subplots()
ax.imshow(image)
plt.show()
def build_block_diagram(boat, controller):
builder = DiagramBuilder()
boat = builder.AddSystem(boat)
boat.set_name('boat')
# logger to track trajectories
logger = LogOutput(boat.get_output_port(0), builder)
logger.set_name('logger')
# expose boats input as an input port
builder.ExportInput(boat.get_input_port(0))
# build diagram
diagram = builder.Build()
diagram.set_name('diagram')
return diagram
def MakeManipulationStation(time_step=0.002,
plant_setup_callback=None,
camera_prefix="camera"):
"""
Sets up a manipulation station with the iiwa + wsg + controllers [+
cameras]. Cameras will be added to each body with a name starting with
"camera".
Args:
time_step: the standard MultibodyPlant time step.
plant_setup_callback: should be a python callable that takes one
argument: `plant_setup_callback(plant)`. It will be called after the
iiwa and WSG are added to the plant, but before the plant is
finalized. Use this to add additional geometry.
camera_prefix: Any bodies in the plant (created during the
plant_setup_callback) starting with this prefix will get a camera
attached.
"""
builder = DiagramBuilder()
# Add (only) the iiwa, WSG, and cameras to the scene.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=time_step)
iiwa = AddIiwa(plant)
wsg = AddWsg(plant, iiwa)
if plant_setup_callback:
plant_setup_callback(plant)
plant.Finalize()
num_iiwa_positions = plant.num_positions(iiwa)
# I need a PassThrough system so that I can export the input port.
iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions))
builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position")
builder.ExportOutput(iiwa_position.get_output_port(),
"iiwa_position_command")
# Export the iiwa "state" outputs.
demux = builder.AddSystem(
Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions))
builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port())
builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured")
builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated")
builder.ExportOutput(plant.get_state_output_port(iiwa),
"iiwa_state_estimated")
# Make the plant for the iiwa controller to use.
controller_plant = MultibodyPlant(time_step=time_step)
controller_iiwa = AddIiwa(controller_plant)
AddWsg(controller_plant, controller_iiwa, welded=True)
controller_plant.Finalize()
# Add the iiwa controller
iiwa_controller = builder.AddSystem(
InverseDynamicsController(controller_plant,
kp=[100] * num_iiwa_positions,
ki=[1] * num_iiwa_positions,
kd=[20] * num_iiwa_positions,
has_reference_acceleration=False))
iiwa_controller.set_name("iiwa_controller")
builder.Connect(plant.get_state_output_port(iiwa),
iiwa_controller.get_input_port_estimated_state())
# Add in the feed-forward torque
adder = builder.AddSystem(Adder(2, num_iiwa_positions))
builder.Connect(iiwa_controller.get_output_port_control(),
adder.get_input_port(0))
# Use a PassThrough to make the port optional (it will provide zero values
# if not connected).
torque_passthrough = builder.AddSystem(PassThrough([0]
* num_iiwa_positions))
builder.Connect(torque_passthrough.get_output_port(),
adder.get_input_port(1))
builder.ExportInput(torque_passthrough.get_input_port(),
"iiwa_feedforward_torque")
builder.Connect(adder.get_output_port(),
plant.get_actuation_input_port(iiwa))
# Add discrete derivative to command velocities.
desired_state_from_position = builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
num_iiwa_positions, time_step, suppress_initial_transient=True))
desired_state_from_position.set_name("desired_state_from_position")
builder.Connect(desired_state_from_position.get_output_port(),
iiwa_controller.get_input_port_desired_state())
builder.Connect(iiwa_position.get_output_port(),
desired_state_from_position.get_input_port())
# Export commanded torques.
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded")
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured")
builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa),
"iiwa_torque_external")
# Wsg controller.
wsg_controller = builder.AddSystem(SchunkWsgPositionController())
wsg_controller.set_name("wsg_controller")
builder.Connect(wsg_controller.get_generalized_force_output_port(),
plant.get_actuation_input_port(wsg))
builder.Connect(plant.get_state_output_port(wsg),
wsg_controller.get_state_input_port())
builder.ExportInput(wsg_controller.get_desired_position_input_port(),
"wsg_position")
builder.ExportInput(wsg_controller.get_force_limit_input_port(),
"wsg_force_limit")
wsg_mbp_state_to_wsg_state = builder.AddSystem(
MakeMultibodyStateToWsgStateSystem())
builder.Connect(plant.get_state_output_port(wsg),
wsg_mbp_state_to_wsg_state.get_input_port())
builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(),
"wsg_state_measured")
builder.ExportOutput(wsg_controller.get_grip_force_output_port(),
"wsg_force_measured")
# Cameras.
AddRgbdSensors(builder,
plant,
scene_graph,
model_instance_prefix=camera_prefix)
# Export "cheat" ports.
builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query")
builder.ExportOutput(plant.get_contact_results_output_port(),
"contact_results")
builder.ExportOutput(plant.get_state_output_port(),
"plant_continuous_state")
builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses")
diagram = builder.Build()
diagram.set_name("ManipulationStation")
return diagram
from pydrake.all import (AddMultibodyPlantSceneGraph,
DiagramBuilder,
Parser,
Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
def make_box_flipup(generator,
observations="state",
meshcat=None,
time_limit=10):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
# TODO(russt): randomize parameters.
box = AddPlanarBinAndSimpleBox(plant)
finger = AddPointFinger(plant)
plant.Finalize()
plant.set_name("plant")
SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id())
controller_plant = MultibodyPlant(time_step=0.005)
AddPointFinger(controller_plant)
if meshcat:
MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3)
ContactVisualizer.AddToBuilder(
builder, plant, meshcat,
ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0))
# Use the controller plant to visualize the set point geometry.
controller_scene_graph = builder.AddSystem(SceneGraph())
controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph)
SetColor(controller_scene_graph,
color=[1.0, 165.0 / 255, 0.0, 1.0],
source_id=controller_plant.get_source_id())
controller_vis = MeshcatVisualizerCpp.AddToBuilder(
builder, controller_scene_graph, meshcat,
MeshcatVisualizerParams(prefix="controller"))
controller_vis.set_name("controller meshcat")
controller_plant.Finalize()
# Stiffness control. (For a point finger with unit mass, the
# InverseDynamicsController is identical)
N = controller_plant.num_positions()
kp = [100] * N
ki = [1] * N
kd = [2 * np.sqrt(kp[0])] * N
controller = builder.AddSystem(
InverseDynamicsController(controller_plant, kp, ki, kd, False))
builder.Connect(plant.get_state_output_port(finger),
controller.get_input_port_estimated_state())
actions = builder.AddSystem(PassThrough(N))
positions_to_state = builder.AddSystem(Multiplexer([N, N]))
builder.Connect(actions.get_output_port(),
positions_to_state.get_input_port(0))
zeros = builder.AddSystem(ConstantVectorSource([0] * N))
builder.Connect(zeros.get_output_port(),
positions_to_state.get_input_port(1))
builder.Connect(positions_to_state.get_output_port(),
controller.get_input_port_desired_state())
builder.Connect(controller.get_output_port(),
plant.get_actuation_input_port())
if meshcat:
positions_to_poses = builder.AddSystem(
MultibodyPositionToGeometryPose(controller_plant))
builder.Connect(
positions_to_poses.get_output_port(),
controller_scene_graph.get_source_pose_port(
controller_plant.get_source_id()))
builder.ExportInput(actions.get_input_port(), "actions")
if observations == "state":
builder.ExportOutput(plant.get_state_output_port(), "observations")
# TODO(russt): Add 'time', and 'keypoints'
else:
raise ValueError("observations must be one of ['state']")
class RewardSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("box_state", 6)
self.DeclareVectorInputPort("finger_state", 4)
self.DeclareVectorInputPort("actions", 2)
self.DeclareVectorOutputPort("reward", 1, self.CalcReward)
def CalcReward(self, context, output):
box_state = self.get_input_port(0).Eval(context)
finger_state = self.get_input_port(1).Eval(context)
actions = self.get_input_port(2).Eval(context)
angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2
cost = 2 * angle_from_vertical**2 # box angle
cost += 0.1 * box_state[5]**2 # box velocity
effort = actions - finger_state[:2]
cost += 0.1 * effort.dot(effort) # effort
# finger velocity
cost += 0.1 * finger_state[2:].dot(finger_state[2:])
# Add 10 to make rewards positive (to avoid rewarding simulator
# crashes).
output[0] = 10 - cost
reward = builder.AddSystem(RewardSystem())
builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0))
builder.Connect(plant.get_state_output_port(finger),
reward.get_input_port(1))
builder.Connect(actions.get_output_port(), reward.get_input_port(2))
builder.ExportOutput(reward.get_output_port(), "reward")
# Set random state distributions.
uniform_random = Variable(name="uniform_random",
type=Variable.Type.RANDOM_UNIFORM)
box_joint = plant.GetJointByName("box")
x, y = box_joint.get_default_translation()
box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0)
diagram = builder.Build()
simulator = Simulator(diagram)
# Termination conditions:
def monitor(context):
if context.get_time() > time_limit:
return EventStatus.ReachedTermination(diagram, "time limit")
return EventStatus.Succeeded()
simulator.set_monitor(monitor)
return simulator
from pydrake.all import (BasicVector, DiagramBuilder, FloatingBaseType,
RigidBodyPlant, RigidBodyTree, Simulator)
from underactuated import (FindResource, PlanarRigidBodyVisualizer)
# Load the double pendulum from Universal Robot Description Format
tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
builder.ExportInput(robot.get_input_port(0))
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
# Set the initial conditions
context = simulator.get_mutable_context()
context.FixInputPort(0, BasicVector([0., 0.])) # Zero input torques
state = context.get_mutable_continuous_state_vector()
state.SetFromVector((1., 1., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(10)
def makeDiagram(n_quadrotors,
n_balls,
use_visualizer=False,
trajectory_u=None,
trajectory_x=None,
trajectory_K=None):
builder = DiagramBuilder()
# Setup quadrotor plants and controllers
quadrotor_plants = []
quadrotor_controllers = []
for i in range(n_quadrotors):
new_quad = Quadrotor2D(n_quadrotors=n_quadrotors - 1, n_balls=n_balls)
new_quad.set_name('quad_' + str(i))
plant = builder.AddSystem(new_quad)
quadrotor_plants.append(plant)
# Setup ball plants
ball_plants = []
for i in range(n_balls):
new_ball = Ball2D(n_quadrotors=n_quadrotors, n_balls=n_balls - 1)
new_ball.set_name('ball_' + str(i))
plant = builder.AddSystem(new_ball)
ball_plants.append(plant)
# Connect all plants so that each object (quadrotor or ball) has access to all other object states as inputs
for i in range(n_quadrotors):
for j in range(n_quadrotors):
if i == j:
continue
k = j if j < i else j - 1
builder.Connect(quadrotor_plants[j].get_output_port(0),
quadrotor_plants[i].GetInputPort('quad_' + str(k)))
for j in range(n_balls):
builder.Connect(ball_plants[j].get_output_port(0),
quadrotor_plants[i].GetInputPort('ball_' + str(j)))
for i in range(n_balls):
for j in range(n_quadrotors):
builder.Connect(quadrotor_plants[j].get_output_port(0),
ball_plants[i].GetInputPort('quad_' + str(j)))
for j in range(n_balls):
if i == j:
continue
k = j if j < i else j - 1
builder.Connect(ball_plants[j].get_output_port(0),
ball_plants[i].GetInputPort('ball_' + str(k)))
# Setup visualization
if use_visualizer:
visualizer = builder.AddSystem(
Visualizer(n_quadrotors=n_quadrotors, n_balls=n_balls))
visualizer.set_name('visualizer')
for i in range(n_quadrotors):
builder.Connect(quadrotor_plants[i].get_output_port(0),
visualizer.get_input_port(i))
for i in range(n_balls):
builder.Connect(ball_plants[i].get_output_port(0),
visualizer.get_input_port(n_quadrotors + i))
# Setup trajectory source
if trajectory_x is not None and trajectory_u is not None and trajectory_K is not None:
demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2))
demulti_u.set_name('feedforward input')
demulti_x = builder.AddSystem(Demultiplexer(6 * n_quadrotors, 6))
demulti_x.set_name('reference trajectory')
demulti_K = builder.AddSystem(Demultiplexer(12 * n_quadrotors, 12))
demulti_K.set_name('time-varying K')
for i in range(n_quadrotors):
ltv_lqr = builder.AddSystem(LTVController(6, 2))
ltv_lqr.set_name('LTV LQR ' + str(i))
builder.Connect(demulti_x.get_output_port(i),
ltv_lqr.get_input_port(0))
builder.Connect(quadrotor_plants[i].get_output_port(0),
ltv_lqr.get_input_port(1))
builder.Connect(demulti_u.get_output_port(i),
ltv_lqr.get_input_port(2))
builder.Connect(demulti_K.get_output_port(i),
ltv_lqr.get_input_port(3))
builder.Connect(ltv_lqr.get_output_port(0),
quadrotor_plants[i].get_input_port(0))
source_u = builder.AddSystem(TrajectorySource(trajectory_u))
source_u.set_name('source feedforward input trajectory')
source_x = builder.AddSystem(TrajectorySource(trajectory_x))
source_x.set_name('source reference trajectory')
demulti_source_x = builder.AddSystem(
Demultiplexer([6 * n_quadrotors, 4 * n_balls]))
demulti_source_x.set_name('quad and ball trajectories')
source_K = builder.AddSystem(TrajectorySource(trajectory_K))
source_K.set_name('source time-varying K')
builder.Connect(source_u.get_output_port(0),
demulti_u.get_input_port(0))
builder.Connect(source_x.get_output_port(0),
demulti_source_x.get_input_port(0))
builder.Connect(demulti_source_x.get_output_port(0),
demulti_x.get_input_port(0))
builder.Connect(source_K.get_output_port(0),
demulti_K.get_input_port(0))
else:
demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2))
demulti_u.set_name('quadrotor input')
for i in range(n_quadrotors):
builder.Connect(demulti_u.get_output_port(i),
quadrotor_plants[i].get_input_port(0))
builder.ExportInput(demulti_u.get_input_port(0))
diagram = builder.Build()
return diagram
class Juggler:
def __init__(self, kp=100, ki=1, kd=20, time_step=0.001, show_axis=False):
"""
Robotic Kuka IIWA juggler with paddle end effector
Args:
kp (int, optional): proportional gain. Defaults to 100.
ki (int, optional): integral gain. Defaults to 1.
kd (int, optional): derivative gain. Defaults to 20.
time_step (float, optional): time step for internal manipulation station controller. Defaults to 0.001.
show_axis (boolean, optional): whether or not to show the axis of reflection.
"""
self.time = 0
juggler_station = JugglerStation(kp=kp,
ki=ki,
kd=kd,
time_step=time_step,
show_axis=show_axis)
self.builder = DiagramBuilder()
self.station = self.builder.AddSystem(juggler_station.get_diagram())
self.position_log = []
self.velocity_log = []
self.visualizer = ConnectMeshcatVisualizer(
self.builder,
output_port=self.station.GetOutputPort("geometry_query"),
zmq_url=zmq_url)
self.plant = juggler_station.get_multibody_plant()
# ---------------------
ik_sys = self.builder.AddSystem(InverseKinematics(self.plant))
ik_sys.set_name("ik_sys")
v_mirror = self.builder.AddSystem(VelocityMirror(self.plant))
v_mirror.set_name("v_mirror")
w_tilt = self.builder.AddSystem(AngularVelocityTilt(self.plant))
w_tilt.set_name("w_tilt")
integrator = self.builder.AddSystem(Integrator(7))
self.builder.Connect(
self.station.GetOutputPort("iiwa_position_measured"),
ik_sys.GetInputPort("iiwa_pos_measured"))
self.builder.Connect(ik_sys.get_output_port(),
integrator.get_input_port())
self.builder.Connect(integrator.get_output_port(),
self.station.GetInputPort("iiwa_position"))
self.builder.Connect(
self.station.GetOutputPort("iiwa_position_measured"),
w_tilt.GetInputPort("iiwa_pos_measured"))
self.builder.Connect(
self.station.GetOutputPort("iiwa_position_measured"),
v_mirror.GetInputPort("iiwa_pos_measured"))
self.builder.Connect(
self.station.GetOutputPort("iiwa_velocity_estimated"),
v_mirror.GetInputPort("iiwa_velocity_estimated"))
self.builder.Connect(
w_tilt.get_output_port(),
ik_sys.GetInputPort("paddle_desired_angular_velocity"))
self.builder.Connect(v_mirror.get_output_port(),
ik_sys.GetInputPort("paddle_desired_velocity"))
self.builder.ExportInput(v_mirror.GetInputPort("ball_pose"),
"v_ball_pose")
self.builder.ExportInput(v_mirror.GetInputPort("ball_velocity"),
"v_ball_velocity")
self.builder.ExportInput(w_tilt.GetInputPort("ball_pose"),
"w_ball_pose")
self.builder.ExportInput(w_tilt.GetInputPort("ball_velocity"),
"w_ball_velocity")
# Useful for debugging
# desired_vel = self.builder.AddSystem(ConstantVectorSource([0, 0, 0]))
# self.builder.Connect(desired_vel.get_output_port(), ik_sys.GetInputPort("paddle_desired_angular_velocity"))
# ---------------------
self.diagram = self.builder.Build()
self.simulator = Simulator(self.diagram)
self.simulator.set_target_realtime_rate(1.0)
self.context = self.simulator.get_context()
self.station_context = self.station.GetMyContextFromRoot(self.context)
self.plant_context = self.plant.GetMyContextFromRoot(self.context)
self.plant.SetPositions(
self.plant_context, self.plant.GetModelInstanceByName("iiwa7"),
[0, np.pi / 3, 0, -np.pi / 2, 0, -np.pi / 3, 0])
self.station.GetInputPort("iiwa_feedforward_torque").FixValue(
self.station_context, np.zeros((7, 1)))
iiwa_model_instance = self.plant.GetModelInstanceByName("iiwa7")
iiwa_q = self.plant.GetPositions(self.plant_context,
iiwa_model_instance)
integrator.GetMyContextFromRoot(
self.context).get_mutable_continuous_state_vector().SetFromVector(
iiwa_q)
def step(self,
simulate=True,
duration=0.1,
final=True,
verbose=False,
display_pose=False):
"""
step the closed loop system
Args:
simulate (bool, optional): whether or not to visualize the command. Defaults to True.
duration (float, optional): duration to complete command in simulation. Defaults to 0.1.
final (bool, optional): whether or not this is the final command in the sequence; relevant for recording. Defaults to True.
verbose (bool, optional): whether or not to print measured position change. Defaults to False.
display_pose (bool, optional): whether or not to show the pose of the paddle in simulation. Defaults to False.
"""
ball_pose = self.plant.EvalBodyPoseInWorld(
self.plant_context,
self.plant.GetBodyByName("ball")).translation()
ball_velocity = self.plant.EvalBodySpatialVelocityInWorld(
self.plant_context,
self.plant.GetBodyByName("ball")).translational()
self.diagram.GetInputPort("v_ball_pose").FixValue(
self.context, ball_pose)
self.diagram.GetInputPort("v_ball_velocity").FixValue(
self.context, ball_velocity)
self.diagram.GetInputPort("w_ball_pose").FixValue(
self.context, ball_pose)
self.diagram.GetInputPort("w_ball_velocity").FixValue(
self.context, ball_velocity)
if display_pose:
transform = self.plant.EvalBodyPoseInWorld(
self.plant_context,
self.plant.GetBodyByName("base_link")).GetAsMatrix4()
AddTriad(self.visualizer.vis,
name=f"paddle_{round(self.time, 1)}",
prefix='',
length=0.15,
radius=.006)
self.visualizer.vis[''][
f"paddle_{round(self.time, 1)}"].set_transform(transform)
if simulate:
self.visualizer.start_recording()
self.simulator.AdvanceTo(self.time + duration)
self.visualizer.stop_recording()
self.position_log.append(
self.station.GetOutputPort("iiwa_position_measured").Eval(
self.station_context))
self.velocity_log.append(
self.station.GetOutputPort("iiwa_velocity_estimated").Eval(
self.station_context))
if verbose:
print("Time: {}\nMeasured Position: {}\n\n".format(
round(self.time, 3),
np.around(
self.station.GetOutputPort(
"iiwa_position_measured").Eval(
self.station_context), 3)))
if len(self.position_log) >= 2 and np.equal(
np.around(self.position_log[-1], 3),
np.around(self.position_log[-2], 3)).all():
print("something went wrong, simulation terminated")
final = True
if final:
self.visualizer.publish_recording()
return self.time + duration
self.time += duration
