def Simulate2dRamone(x0, duration,
desired_goal = 0.0,
print_period = 0.0):
builder = DiagramBuilder()
tree = RigidBodyTree()
AddModelInstanceFromUrdfFile("ramone_act.urdf", FloatingBaseType.kRollPitchYaw, None, tree)
plant = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(
Ramone2dController(tree,
desired_goal=desired_goal,
print_period=print_period))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
simulator.get_mutable_context().set_accuracy(1e-4)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector(x0)
simulator.StepTo(duration)
return tree, controller, state_log, plant
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port,
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(
plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
body = add_body(plant, "welded_body")
add_geometry(plant, body)
plant.WeldFrames(plant.world_frame(), body.body_frame())
body = add_body(plant, "floating_body")
add_geometry(plant, body)
plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.]))
plant.Finalize()
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_context()
# plant.SetFreeBodyPose(context, body, X_WB)
# plant.SetFreeBodySpatialVelocity(body, V_WB, context)
# Should look at, show 40sec to 50sec.
# TODO(eric.cousineau): Without reset stats, it freezes? :(
# simulator.AdvanceTo(40.)
simulator.set_target_realtime_rate(100.)
# simulator.ResetStatistics()
dt = 0.1
while context.get_time() < 240.:
simulator.AdvanceTo(context.get_time() + dt)
def playbackMotion(data1, data2, data3, data4, times):
data = np.concatenate((data2, data1, data4, data3), axis=0)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(Player(data, times))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
#print(robot.get_output_port(0).size())
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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(True)
# Simulate for 10 seconds
simulator.StepTo(times[-1] + 0.5)
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
Tview=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=1e-3))
station.SetupClutterClearingStation()
if manipuland is "foam":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
T_world_objectInitial)
elif manipuland is "ball":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/sphere.sdf",
T_world_objectInitial)
elif manipuland is "bball":
station.AddManipulandFromFile(
"drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
T_world_objectInitial)
elif manipuland is "rod":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/rod.sdf",
T_world_objectInitial)
station_plant = station.get_multibody_plant()
parser = Parser(station_plant)
if include_bin:
parser.AddModelFromFile("extra_bin.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
if include_hoop:
parser.AddModelFromFile("sdfs/hoop.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
station.Finalize()
frames_to_draw = {"gripper": {"body"}}
meshcat = None
if zmq_url is not None:
meshcat = ConnectMeshcatVisualizer(builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
delete_prefix_on_load=False,
frames_to_draw=frames_to_draw,
zmq_url=zmq_url)
diagram = builder.Build()
plant = station.get_multibody_plant()
context = plant.CreateDefaultContext()
gripper = plant.GetBodyByName("body")
initial_pose = plant.EvalBodyPoseInWorld(context, gripper)
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return initial_pose, meshcat
def simulateRobot(time, B, v_command):
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(DController(tree, B, v_command))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def simulate_acrobot():
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * math.pi)
wrapangles.set_interval(1, -math.pi, math.pi)
wrapto = builder.AddSystem(wrapangles)
builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0))
controller = builder.AddSystem(BalancingLQR())
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
parser = argparse.ArgumentParser()
parser.add_argument("-N",
"--trials",
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
args = parser.parse_args()
print(AcrobotPlant)
for i in range(args.trials):
context.set_time(0.)
state.SetFromVector(UprightState().CopyToVector() +
0.05 * np.random.randn(4, ))
simulator.StepTo(args.duration)
def RunSimulation(self, real_time_rate=1.0):
'''
Here we test using the NNSystem in a Simulator to drive
an acrobot.
'''
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def simulate_scene_tree(scene_tree, T, timestep=0.001, with_meshcat=False):
builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg(
scene_tree, timestep=timestep)
mbp.Finalize()
if with_meshcat:
visualizer = ConnectMeshcatVisualizer(builder, scene_graph,
zmq_url="default")
diagram = builder.Build()
diag_context = diagram.CreateDefaultContext()
sim = Simulator(diagram)
sim.set_target_realtime_rate(1.0)
sim.AdvanceTo(T)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("-t1", default=0.05, help="Extend leg")
parser.add_argument("-t2", default=0.5, help="Dwell at top")
parser.add_argument("-t3", default=0.5, help="Contract leg")
parser.add_argument("-t4", default=0.1, help="Wait at bottom")
setup_argparse_for_setup_dot_diagram(parser)
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
t1 = float(args.t1)
t2 = float(args.t2)
t3 = float(args.t3)
t4 = float(args.t4)
q_crouch = np.array([
1600, 2100, 2000, 1600, 2100, 2000, 1400, 2100, 2000, 1400, 2100, 2000
])
q_extend = np.array([
1600, 1600, 2400, 1600, 1600, 2400, 1400, 1600, 2400, 1400, 1600, 2400
])
breaks = np.cumsum([0., t1, t2, t3, t4])
samples = np.stack([q_crouch, q_extend, q_extend, q_crouch, q_crouch]).T
trajectory = PiecewisePolynomial.FirstOrderHold(breaks, samples)
builder = DiagramBuilder()
plant, scene_graph, servo_controller = setup_dot_diagram(builder, args)
trajectory_source = builder.AddSystem(TrajectoryLooper(trajectory))
builder.Connect(trajectory_source.get_output_port(0),
servo_controller.get_input_port(0))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder,
output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat,
open_browser=args.open_browser)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
def jacobian_controller_example():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
controller = builder.AddSystem(
PseudoInverseController(station.get_multibody_plant()))
integrator = builder.AddSystem(Integrator(7))
builder.Connect(controller.get_output_port(), integrator.get_input_port())
builder.Connect(integrator.get_output_port(),
station.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
controller.get_input_port())
meshcat = ConnectMeshcatVisualizer(
builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"))
diagram = builder.Build()
simulator = Simulator(diagram)
station_context = station.GetMyContextFromRoot(
simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros((7, 1)))
station.GetInputPort("wsg_position").FixValue(station_context, [0.1])
integrator.GetMyContextFromRoot(simulator.get_mutable_context(
)).get_mutable_continuous_state_vector().SetFromVector(
station.GetIiwaPosition(station_context))
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return simulator
def runManipulationExample(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupDefaultStation()
station.Finalize()
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
pose_bundle_output_port = station.GetOutputPort("pose_bundle")
# Side-on view of the station.
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
scene_graph, Tview=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
draw_period=0.1))
builder.Connect(pose_bundle_output_port,
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the control inputs to zero.
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_position").FixValue(
station_context, station.GetIiwaPosition(station_context))
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
station.GetInputPort("wsg_position").FixValue(
station_context, np.zeros(1))
station.GetInputPort("wsg_force_limit").FixValue(
station_context, [40.0])
simulator.StepTo(args.duration)
def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
path = subprocess.check_output([
'venv/share/drake/common/resource_tool',
'-print_resource_path',
'drake/examples/multibody/cart_pole/cart_pole.sdf',
]).strip()
Parser(plant).AddModelFromFile(path)
plant.Finalize()
# Add to visualizer.
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Add controller.
controller = builder.AddSystem(BalancingLQR(plant))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
# Set up a simulator to run this diagram.
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
# Simulate.
duration = 8.0
for i in range(5):
initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4)
print(f"Iteration: {i}. Initial: {initial_state}")
context.SetContinuousState(initial_state)
context.SetTime(0.0)
simulator.Initialize()
simulator.AdvanceTo(duration)
def main():
parser = argparse.ArgumentParser(description=__doc__)
setup_argparse_for_setup_dot_diagram(parser)
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
q_init = np.array([
1700, 2000, 2000, 1700, 2000, 2000, 1300, 2000, 2000, 1300, 2000, 2000
])
builder = DiagramBuilder()
plant, scene_graph, servo_controller = setup_dot_diagram(builder, args)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(ServoSliders(servo_controller))
sliders.set_position(q_init)
builder.Connect(sliders.get_output_port(0),
servo_controller.get_input_port(0))
else:
source = builder.AddSystem(
ConstantVectorSource(np.zeros(servo_controller.nu)))
builder.Connect(source.get_output_port(0),
servo_controller.get_input_port(0))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder,
output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat,
open_browser=args.open_browser)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
def getPredictedMotion(B, v_command, time):
#object_positions = object_positions + 0.1
#manipulator_positions = manipulator_positions + 0.1
#object_velocities = object_velocities + 0.1
#manipulator_velocities = manipulator_velocities + 0.1
#object_positions = object_positions[:, range(0,object_positions.shape[1],2)]
#manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)]
#object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)]
#manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)]
#times = times[range(0,times.size,2)]
#import pdb; pdb.set_trace()
step = 0.01
A = 10 * np.eye(3)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
#robot = builder.AddSystem(RigidBodyPlant(tree))
robot = builder.AddSystem(
QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B)))
controller = builder.AddSystem(QController(tree, v_command, step))
#builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_discrete_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
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
def BuildAndSimulateTrajectory(
q_traj,
g_traj,
get_gripper_controller,
T_world_objectInitial,
T_world_targetBin,
zmq_url,
time_step,
include_target_bin=True,
include_hoop=False,
manipuland="foam"
):
"""Simulate trajectory for manipulation station.
@param q_traj: Trajectory class used to initialize TrajectorySource for joints.
@param g_traj: Trajectory class used to initialize TrajectorySource for gripper.
"""
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=time_step)) #1e-3 or 1e-4 probably
station.SetupClutterClearingStation()
if manipuland is "foam":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
T_world_objectInitial)
elif manipuland is "ball":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/sphere.sdf",
T_world_objectInitial)
elif manipuland is "bball":
station.AddManipulandFromFile(
"drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
T_world_objectInitial)
elif manipuland is "rod":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/rod.sdf",
T_world_objectInitial)
station_plant = station.get_multibody_plant()
parser = Parser(station_plant)
if include_target_bin:
parser.AddModelFromFile("sdfs/extra_bin.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
if include_hoop:
parser.AddModelFromFile("sdfs/hoop.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
station.Finalize()
# iiwa joint trajectory - predetermined trajectory
q_traj_system = builder.AddSystem(TrajectorySource(q_traj))
builder.Connect(q_traj_system.get_output_port(),
station.GetInputPort("iiwa_position"))
# gripper - closed loop controller
gctlr = builder.AddSystem(get_gripper_controller(station_plant))
gctlr.set_name("GripperControllerUsingIiwaState")
builder.Connect(station.GetOutputPort("iiwa_position_measured"), gctlr.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), gctlr.GetInputPort("iiwa_velocity"))
builder.Connect(gctlr.get_output_port(), station.GetInputPort("wsg_position"))
loggers = dict(
state=builder.AddSystem(SignalLogger(31)),
v_est=builder.AddSystem(SignalLogger(7))
)
builder.Connect(station.GetOutputPort("plant_continuous_state"),
loggers["state"].get_input_port())
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
loggers["v_est"].get_input_port())
meshcat = None
if zmq_url is not None:
meshcat = ConnectMeshcatVisualizer(builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
delete_prefix_on_load=True,
frames_to_draw={"gripper":{"body"}},
zmq_url=zmq_url)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
return simulator, station_plant, meshcat, loggers
assert n == 1
q_default.append(default_positions[0])
return q_default
q_iiwa0 = get_default_joint_positions(model_iiwa)
gripper_position_input_port = station.GetInputPort("wsg_position")
gripper_position_input_port.FixValue(context_station, [0.05])
iiwa_position_input_port = station.GetInputPort("iiwa_position")
iiwa_position_input_port.FixValue(context_station, q_iiwa0)
for name, model_list in models_object.items():
n_objects = len(model_names)
x = 0.4 + 0.2 * np.random.rand(n_objects)
y = -0.2 + 0.2 * np.random.rand(n_objects)
z = 0.4 + 0.6 * np.random.rand(n_objects)
X_WObject_list = [
RigidTransform(np.array([x[i], y[i], z[i]])) for i in range(n_objects)
]
for i, model in enumerate(model_list):
plant.SetFreeBodyPose(context_plant,
plant.get_body(plant.GetBodyIndices(model)[0]),
X_WObject_list[i])
#%%
sim = Simulator(diagram, context)
sim.set_target_realtime_rate(1.0)
sim.AdvanceTo(2.0)
def RunSimulation(robobee_plantBS,
control_law,
x0=np.random.random((15, 1)),
duration=30):
robobee_controller = RobobeeController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
RobobeePlantBS(m=robobee_plantBS.m,
Ixx=robobee_plantBS.Ixx,
Iyy=robobee_plantBS.Iyy,
Izz=robobee_plantBS.Izz,
g=robobee_plantBS.g,
input_max=robobee_plantBS.input_max))
Rigidbody_selector = builder.AddSystem(RigidBodySelection())
print("1. Connecting plant and controller\n")
controller = builder.AddSystem(robobee_controller)
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to capture the simulation of our plant
print("2. Connecting plant to the logger\n")
set_time_interval = 0.001
time_interval_multiple = 1000
publish_period = set_time_interval * time_interval_multiple
input_log = builder.AddSystem(SignalLogger(4))
# input_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(15))
# state_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Drake visualization
print("3. Connecting plant output to DrakeVisualizer\n")
rtree = RigidBodyTree(
FindResourceOrThrow("drake/examples/robobee/robobee_arena.urdf"),
FloatingBaseType.kQuaternion) #robobee_twobar
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(plant.get_output_port(0),
Rigidbody_selector.get_input_port(0))
builder.Connect(Rigidbody_selector.get_output_port(0),
visualizer.get_input_port(0))
print("4. Building diagram\n")
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
print("5. Create simulation\n")
simulator = Simulator(diagram, context)
simulator.Initialize()
# simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(0.05)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
builder = DiagramBuilder()
acrobot = builder.AddSystem(RigidBodyPlant(tree))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-4., 4.],
ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.))
builder.Connect(torque_system.get_output_port(0),
acrobot.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([1., 0., 0., 0.])
simulator.StepTo(args.duration)
def project_tree_to_feasibility(tree,
constraints=[],
jitter_q=None,
do_forward_sim=False,
zmq_url=None,
prefix="projection",
timestep=0.001,
T=1.):
# Mutates tree into tree with bodies in closest
# nonpenetrating configuration.
builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \
compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep)
mbp.Finalize()
# Connect visualizer if requested. Wrap carefully to keep it
# from spamming the console.
if zmq_url is not None:
with open(os.devnull, 'w') as devnull:
with contextlib.redirect_stdout(devnull):
visualizer = ConnectMeshcatVisualizer(builder,
sg,
zmq_url=zmq_url,
prefix=prefix)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
q0 = mbp.GetPositions(mbp_context)
nq = len(q0)
if nq == 0:
logging.warn("Generated MBP had no positions.")
return tree
# Set up projection NLP.
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
# It's always a projection, so we always have this
# Euclidean norm error between the optimized q and
# q0.
prog.AddQuadraticErrorCost(np.eye(nq), q0, q_dec)
# Nonpenetration constraint.
ik.AddMinimumDistanceConstraint(0.001)
# Other requested constraints.
for constraint in constraints:
constraint.add_to_ik_prog(tree, ik, mbp, mbp_context,
node_to_free_body_ids_map)
# Initial guess, which can be slightly randomized by request.
q_guess = q0
if jitter_q:
q_guess = q_guess + np.random.normal(0., jitter_q, size=q_guess.size)
prog.SetInitialGuess(q_dec, q_guess)
# Solve.
solver = SnoptSolver()
options = SolverOptions()
logfile = "/tmp/snopt.log"
os.system("rm %s" % logfile)
options.SetOption(solver.id(), "Print file", logfile)
options.SetOption(solver.id(), "Major feasibility tolerance", 1E-3)
options.SetOption(solver.id(), "Major optimality tolerance", 1E-3)
options.SetOption(solver.id(), "Major iterations limit", 300)
result = solver.Solve(prog, None, options)
if not result.is_success():
logging.warn("Projection failed.")
print("Logfile: ")
with open(logfile) as f:
print(f.read())
qf = result.GetSolution(q_dec)
mbp.SetPositions(mbp_context, qf)
# If forward sim is requested, do a quick forward sim to get to
# a statically stable config.
if do_forward_sim:
sim = Simulator(diagram, diagram_context)
sim.set_target_realtime_rate(1000.)
sim.AdvanceTo(T)
# Reload poses back into tree
free_bodies = mbp.GetFloatingBaseBodies()
for body_id, node in body_id_to_node_map.items():
if body_id not in free_bodies:
continue
node.tf = drake_tf_to_torch_tf(
mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id)))
return tree
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
ylim=[-1., 2.],
figsize_multiplier=2))
builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
context.set_accuracy(1e-4)
context.SetContinuousState([0., 0., 0.4, -2.])
simulator.StepTo(args.duration)
def project_tree_to_feasibility_via_sim(tree,
constraints=[],
zmq_url=None,
prefix="projection",
timestep=0.0005,
T=10.):
# Mutates tree into tree with bodies in closest
# nonpenetrating configuration.
builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \
compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep)
mbp.Finalize()
# Connect visualizer if requested. Wrap carefully to keep it
# from spamming the console.
if zmq_url is not None:
with open(os.devnull, 'w') as devnull:
with contextlib.redirect_stdout(devnull):
visualizer = ConnectMeshcatVisualizer(builder,
sg,
zmq_url=zmq_url,
prefix=prefix)
# Forward sim under langevin forces
force_source = builder.AddSystem(
DecayingForceToDesiredConfigSystem(
mbp, mbp.GetPositions(mbp.CreateDefaultContext())))
builder.Connect(mbp.get_state_output_port(),
force_source.get_input_port(0))
builder.Connect(force_source.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
q0 = mbp.GetPositions(mbp_context)
nq = len(q0)
if nq == 0:
logging.warn("Generated MBP had no positions.")
return tree
# Make 'safe' initial configuration that randomly arranges objects vertically
#k = 0
#all_pos = []
#for node in tree:
# for body_id in node_to_free_body_ids_map[node]:
# body = mbp.get_body(body_id)
# tf = mbp.GetFreeBodyPose(mbp_context, body)
# tf = RigidTransform(p=tf.translation() + np.array([0., 0., 1. + k*0.5]), R=tf.rotation())
# mbp.SetFreeBodyPose(mbp_context, body, tf)
# k += 1
sim = Simulator(diagram, diagram_context)
sim.set_target_realtime_rate(1000)
sim.AdvanceTo(T)
# Reload poses back into tree
free_bodies = mbp.GetFloatingBaseBodies()
for body_id, node in body_id_to_node_map.items():
if body_id not in free_bodies:
continue
node.tf = drake_tf_to_torch_tf(
mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id)))
return tree
