def _create_plant(self, urdf):
self.plant = MultibodyPlant(time_step=0.0)
self.scenegraph = SceneGraph()
self.plant.RegisterAsSourceForSceneGraph(self.scenegraph)
self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf))
self.builder = DiagramBuilder()
self.builder.AddSystem(self.scenegraph)
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 visualize(urdf, xtraj):
plant = MultibodyPlant(time_step=0.0)
scenegraph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(self.scenegraph)
model_index = Parser(plant).AddModelFromFile(FindResource(urdf))
builder = DiagramBuilder()
builder.AddSystem(scenegraph)
plant.Finalize()
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 test_visualizer(self):
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
state = GliderState(np.zeros((7,)))
state.x = -.3
state.z = 0.1
state.pitch = 0.2
state.elevator = .4
source = builder.AddSystem(ConstantVectorSource(state[:]))
geom = GliderGeometry.AddToBuilder(builder, source.get_output_port(0),
scene_graph)
# plt_vis = ConnectPlanarSceneGraphVisualizer(builder, scene_graph)
drake_viz = ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("sdf_path", help="path to sdf")
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.01)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant)
model = parser.AddModelFromFile(args.sdf_path)
plant.Finalize()
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
else:
q0 = plant.GetPositions(plant.CreateDefaultContext())
sliders = builder.AddSystem(ConstantVectorSource(q0))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
drake_theta_pub_echo.publish(x_cmd[1,times])
times=times+1
#'current x state is :{}'.format(statex)
rospy.loginfo(u_cmd[:,times])
rate.sleep()
if __name__ == '__main__':
rospy.init_node('drake_control', anonymous=True)
#fig, ax = plt.subplots(2,1,figsize=(8,8))
i=0
sum_x=0
#x_pub = rospy.Publisher('/curretx', catpolexMsg, queue_size=100)
drake_cmd_pub = rospy.Publisher('/chassis_world_effort_controller/command', Float64, queue_size=100)
#drake_cmd_pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=100)
drake_cmd_pub_echo = rospy.Publisher('/drake_cmdu0_pub_echo', Float64, queue_size=100)
drake_state_pub_echo = rospy.Publisher('/drake_x_pub_echo', Float64, queue_size=100)
drake_theta_pub_echo = rospy.Publisher('/drake_theta_pub_echo', Float64, queue_size=100)
#/chassis_world_effort_controller/command
# sub = rospy.Subscriber("/curretx",catpolexMsg,currentCallback, queue_size=1, buff_size=100)#1000HZ
plant = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource("/home/cby/catkin_ws/src/cartpole/urdf/cartpole2.urdf")
xu_cmd=drake_trajectory_generation(file_name)#u_cmd with 400points effort input
u_cmd=u_cmd_drake
x_cmd=x_cmd_drake
rospy.loginfo("trajectory complete!! sending")
drake_cmd_publisher(u_cmd,x_cmd)
#rospy.spin()
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.examples.acrobot import (AcrobotGeometry, AcrobotInput,
AcrobotPlant, AcrobotState)
from underactuated import SliderSystem
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
scene_graph = builder.AddSystem(SceneGraph())
AcrobotGeometry.AddToBuilder(builder, acrobot.get_output_port(0), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
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)
class Visualizer():
def __init__(self, urdf):
self._create_plant(urdf)
def _create_plant(self, urdf):
self.plant = MultibodyPlant(time_step=0.0)
self.scenegraph = SceneGraph()
self.plant.RegisterAsSourceForSceneGraph(self.scenegraph)
self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf))
self.builder = DiagramBuilder()
self.builder.AddSystem(self.scenegraph)
def _finalize_plant(self):
self.plant.Finalize()
def _add_trajectory_source(self, traj):
# Create the trajectory source
source = self.builder.AddSystem(TrajectorySource(traj))
pos2pose = self.builder.AddSystem(MultibodyPositionToGeometryPose(self.plant, input_multibody_state=True))
# Wire the source to the scene graph
self.builder.Connect(source.get_output_port(0), pos2pose.get_input_port())
self.builder.Connect(pos2pose.get_output_port(), self.scenegraph.get_source_pose_port(self.plant.get_source_id()))
def _add_renderer(self):
renderer_name = "renderer"
self.scenegraph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams()))
# Add a camera
depth_camera = DepthRenderCamera(
RenderCameraCore(
renderer_name,
CameraInfo(width=640, height=480, fov_y=np.pi/4),
ClippingRange(0.01, 10.0),
RigidTransform()),
DepthRange(0.01,10.0)
)
world_id = self.plant.GetBodyFrameIdOrThrow(self.plant.world_body().index())
X_WB = xyz_rpy_deg([4,0,0],[-90,0,90])
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
self.builder.AddSystem(sensor)
self.builder.Connect(self.scenegraph.get_query_output_port(), sensor.query_object_input_port())
def _connect_visualizer(self):
DrakeVisualizer.AddToBuilder(self.builder, self.scenegraph)
self.meshcat = ConnectMeshcatVisualizer(self.builder, self.scenegraph, zmq_url="new", open_browser=False)
self.meshcat.vis.jupyter_cell()
def _make_visualization(self, stop_time):
simulator = Simulator(self.builder.Build())
simulator.Initialize()
self.meshcat.vis.render_static()
# Set simulator context
simulator.get_mutable_context().SetTime(0.0)
# Start recording and simulate the diagram
self.meshcat.reset_recording()
self.meshcat.start_recording()
simulator.AdvanceTo(stop_time)
# Publish the recording
self.meshcat.publish_recording()
# Render
self.meshcat.vis.render_static()
input("View visualization. Press <ENTER> to end")
print("Finished")
def visualize_trajectory(self, xtraj=None):
self._finalize_plant()
print("Adding trajectory source")
xtraj = self._check_trajectory(xtraj)
self._add_trajectory_source(xtraj)
print("Adding renderer")
self._add_renderer()
print("Connecting to MeshCat")
self._connect_visualizer()
print("Making visualization")
self._make_visualization(xtraj.end_time())
def _check_trajectory(self, traj):
if traj is None:
plant_context = self.plant.CreateDefaultContext()
pose = self.plant.GetPositions(plant_context)
pose = np.column_stack((pose, pose))
pose = zero_pad_rows(pose, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold([0., 1.], pose)
elif type(traj) is np.ndarray:
if traj.ndim == 1:
traj = np.column_stack(traj, traj)
traj = zero_pad_rows(traj, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold([0.,1.], traj)
elif type(traj) is PiecewisePolynomial:
breaks = traj.get_segment_times()
values = traj.vector_values(breaks)
values = zero_pad_rows(values, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold(breaks, values)
else:
raise ValueError("Trajectory must be a piecewise polynomial, an ndarray, or None")
# stack states
x_opt = np.hstack((q_opt, qd_opt))
"""## Animate the Result
Here we quickly build a Drake diagram to animate the result we got from trajectory optimization: useful for debugging your code and to be sure that everything looks good.
"""
# interpolate state values for animation
time_breaks_opt = np.array([sum(h_opt[:t]) for t in range(T+1)])
x_opt_poly = PiecewisePolynomial.FirstOrderHold(time_breaks_opt, x_opt.T)
# parse urdf with scene graph
compass_gait = MultibodyPlant(time_step=0)
scene_graph = SceneGraph()
compass_gait.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource('models/compass_gait_limit_cycle.urdf')
Parser(compass_gait).AddModelFromFile(file_name)
compass_gait.Finalize()
# build block diagram and drive system state with
# the trajectory from the optimization problem
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_opt_poly))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(compass_gait, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(), scene_graph.get_source_pose_port(compass_gait.get_source_id()))
# add visualizer
import matplotlib.pyplot as plt
import numpy as np
import timeit
# Import utilities from pydrake
from pydrake.common import FindResourceOrThrow
from pydrake.all import (MultibodyPlant, PiecewisePolynomial,
DirectCollocation, DiagramBuilder, SceneGraph,
PlanarSceneGraphVisualizer, Simulator,
TrajectorySource, MultibodyPositionToGeometryPose,
AddMultibodyPlantSceneGraph, Solve, RigidTransform)
from pydrake.multibody.parsing import Parser
from pydrake.solvers.snopt import SnoptSolver
# Create a Multibody plant model from the acrobot
plant = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Find and load the Acrobot URDF
# Note that we cannot include collision geometry in the URDF, otherwise setting up the visualization will fail.
acro_file = FindResourceOrThrow(
"drake/examples/acrobot/Acrobot_no_collision.urdf")
Parser(plant).AddModelFromFile(acro_file)
# Weld the base frame to the world frame
base_frame = plant.GetBodyByName("base_link").body_frame()
world_frame = plant.world_frame()
plant.WeldFrames(world_frame, base_frame, RigidTransform())
# Finalize the plant
plant.Finalize()
# Create the default context
context0 = plant.CreateDefaultContext()
