def get_box_from_geom(scene_graph, visual_only=True):
# TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects
# TODO: get_contact_results_output_port
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# 'link', 'num_links'
#builder.Connect(scene_graph.get_pose_bundle_output_port(),
# viz.get_input_port(0))
# TODO: hash bodies instead
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
# source_name = 'Source_1'
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom,
key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
# string_data is empty...
if visual_only and (geom.color[3] == 0):
continue
# TODO: sort by lowest point on the bounding box?
# TODO: maybe just return the set of bodies in order and let the user decide what to with them
visual_index += 1 # TODO: affected by transparent visual
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height / 2.])
#meshcat_geom = meshcat.geometry.Cylinder(
# geom.float_data[1], geom.float_data[0])
# In Drake, cylinders are along +z
#elif geom.type == geom.MESH:
# meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
# geom.string_data[0:-3] + "obj")
else:
#print("Robot {}, link {}, geometry {}: UNSUPPORTED GEOMETRY TYPE {} WAS IGNORED".format(
# model_index, frame_name, visual_index-1, geom.type))
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsIsometry3() #.GetAsMatrix4()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
for scene_k in range(25):
# Set up a new Drake scene from scratch.
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.005))
# Add "tabletop" (ground) as a fixed Box welded to the world.
world_body = mbp.world_body()
ground_shape = Box(1., 1., 1.)
ground_body = mbp.AddRigidBody("ground", SpatialInertia(
mass=10.0, p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform())
RegisterVisualAndCollisionGeometry(
mbp, ground_body,
RigidTransform(p=[0, 0, -0.5]),
ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]),
CoulombFriction(0.9, 0.8))
# Figure out what YCB objects we have available to add.
ycb_object_dir = os.path.join(
getDrakePath(), "manipulation/models/ycb/sdf/")
ycb_object_sdfs = os.listdir(ycb_object_dir)
ycb_object_sdfs = [os.path.join(ycb_object_dir, path)
for path in ycb_object_sdfs]
# Add random objects to the scene.
parser = Parser(mbp, scene_graph)
def __init__(self, plant, scene_graph, default_joint_angle=-np.pi/60,
damping=1e-5, stiffness=1e-3):
# Drake objects
self.plant = plant
self.scene_graph = scene_graph
# Geometric and physical quantities
# PROGRAMMING: Refactor constants.FRICTION to be constants.mu
self.mu = constants.FRICTION
self.default_joint_angle = default_joint_angle
self.link_width = PLYWOOD_LENGTH/2
self.y_dim = self.link_width * config.num_links.value
self.link_mass = self.x_dim*self.y_dim*self.z_dim*self.density
# Lists of internal Drake objects
self.link_idxs = []
self.joints = []
self.damping = damping
self.stiffness = stiffness
self.instance = self.plant.AddModelInstance(self.name)
for link_num in range(config.num_links.value):
# Initialize bodies and instances
paper_body = self.plant.AddRigidBody(
self.name + "_body" + str(link_num),
self.instance,
SpatialInertia(mass=self.link_mass,
# CoM at origin of body frame
p_PScm_E=np.array([0., 0., 0.]),
# Default moment of inertia for a solid box
G_SP_E=UnitInertia.SolidBox(
self.x_dim, self.link_width, self.z_dim))
)
if self.plant.geometry_source_is_registered():
# Set a box with link dimensions for collision geometry
self.plant.RegisterCollisionGeometry(
paper_body,
RigidTransform(), # Pose in body frame
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim), # Actual shape
self.name + "_body" + str(link_num), pydrake.multibody.plant.CoulombFriction(
self.mu, self.mu) # Friction parameters
)
# Set Set a box with link dimensions for visual geometry
self.plant.RegisterVisualGeometry(
paper_body,
RigidTransform(),
pydrake.geometry.Box(
self.x_dim, self.link_width, self.z_dim),
self.name + "_body" + str(link_num),
[0, 1, 0, 1]) # RGBA color
# Operations between adjacent links
if link_num > 0:
# Get bodies
paper1_body = self.plant.get_body(
BodyIndex(self.link_idxs[-1]))
paper2_body = self.plant.GetBodyByName(
self.name + "_body" + str(link_num), self.instance)
# Set up joints
paper1_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper1_body,
RigidTransform(RotationMatrix(),
[
0,
(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper1_hinge_frame)
paper2_hinge_frame = pydrake.multibody.tree.FixedOffsetFrame(
"paper_hinge_frame",
paper2_body,
RigidTransform(RotationMatrix(),
[
0,
-(self.link_width+self.hinge_diameter)/2,
(self.z_dim+self.hinge_diameter)/2
])
)
self.plant.AddFrame(paper2_hinge_frame)
joint = self.plant.AddJoint(pydrake.multibody.tree.RevoluteJoint(
"paper_hinge_" + str(link_num),
paper1_hinge_frame,
paper2_hinge_frame,
[1, 0, 0],
damping=damping))
if isinstance(default_joint_angle, list):
joint.set_default_angle(
self.default_joint_angle[link_num])
else:
joint.set_default_angle(self.default_joint_angle)
self.plant.AddForceElement(
RevoluteSpring(joint, 0, self.stiffness))
self.joints.append(joint)
# Ignore collisions between adjacent links
geometries = self.plant.CollectRegisteredGeometries(
[paper1_body, paper2_body])
self.scene_graph.collision_filter_manager().Apply(
CollisionFilterDeclaration()
.ExcludeWithin(geometries))
self.link_idxs.append(int(paper_body.index()))
def __init__(self, tf):
geom = PhysicsGeometryInfo(fixed=True)
geom.register_geometry(
drake_tf_to_torch_tf(RigidTransform(p=[0.0, 0., -1.0])),
pydrake_geom.Box(20., 20., 2.))
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
def plan_prethrow_pose(
T_world_robotInitial,
p_world_target, # np.array of shape (3,)
gripper_to_object_dist,
throw_height=0.5, # meters
prethrow_height=0.2,
prethrow_radius=0.4,
throw_angle=np.pi / 4.0,
meshcat=None,
throw_speed_adjustment_factor=1.0,
):
"""
only works with the "back portion" of the clutter station until we figure out how to move the bins around
motion moves along an arc from a "pre throw" to a "throw" position
"""
theta = 1.0 * np.arctan2(p_world_target[1], p_world_target[0])
print(f"theta={theta}")
T_world_prethrow = RigidTransform(
p=np.array([
prethrow_radius * np.cos(theta), prethrow_radius * np.sin(theta),
prethrow_height
]),
R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply(
RotationMatrix.MakeYRotation((theta - np.pi / 2))))
throw_radius = throw_height - prethrow_height
T_world_throw = RigidTransform(
p=T_world_prethrow.translation() + np.array([
throw_radius * np.cos(theta), throw_radius * np.sin(theta),
throw_height - prethrow_height
]),
R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply(
RotationMatrix.MakeYRotation((theta - np.pi / 2)).multiply(
RotationMatrix.MakeXRotation(-np.pi / 2))))
if meshcat:
visualize_transform(meshcat, "T_world_prethrow", T_world_prethrow)
visualize_transform(meshcat, "T_world_throw", T_world_throw)
p_world_object_at_launch = interpolatePosesArcMotion(
T_world_prethrow, T_world_throw, t=throw_angle /
(np.pi / 2.)).translation() + np.array([0, 0, -gripper_to_object_dist])
pdot_world_launch = interpolatePosesArcMotion_pdot(T_world_prethrow,
T_world_throw,
t=throw_angle /
(np.pi / 2.))
launch_speed_base = np.linalg.norm(pdot_world_launch)
launch_speed_required = get_launch_speed_required(
theta=throw_angle,
x=np.linalg.norm(p_world_target[:2]) -
np.linalg.norm(p_world_object_at_launch[:2]),
y=p_world_target[2] - p_world_object_at_launch[2])
total_throw_time = launch_speed_base / launch_speed_required / throw_speed_adjustment_factor
# print(f"p_world_object_at_launch={p_world_object_at_launch}")
# print(f"target={p_world_target}")
# print(f"dx={np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2])}")
# print(f"dy={p_world_target[2] - p_world_object_at_launch[2]}")
# print(f"pdot_world_launch={pdot_world_launch}")
# print(f"total_throw_time={total_throw_time}")
return T_world_prethrow, T_world_throw
def __init__(self, tf):
assert self.sdf is not None, "Don't instantiate ObjectModel itself; use a reified version."
geom = PhysicsGeometryInfo(fixed=False)
geom.register_model_file(
drake_tf_to_torch_tf(RigidTransform(p=[0.0, 0., 0.])), self.sdf)
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
from trajopt.constraints import NCCImplementation, NCCSlackType
import utilities as utils
from scipy.special import erfinv
import pickle
import os
from tempfile import TemporaryFile
from plot_hopper import plot_CC, plot_erm
from pydrake.all import PiecewisePolynomial, RigidTransform
import pickle
# Create the block model with the default flat terrain
_file = "systems/urdf/single_legged_hopper.urdf"
plant = TimeSteppingMultibodyPlant(file=_file)
body_inds = plant.multibody.GetBodyIndices(plant.model_index)
base_frame = plant.multibody.get_body(body_inds[0]).body_frame()
plant.multibody.WeldFrames(plant.multibody.world_frame(), base_frame,
RigidTransform())
plant.Finalize()
num_step = 101
step_size = 0.03
# Get the default context
context = plant.multibody.CreateDefaultContext()
# set chance constraints parameters
# beta_theta = np.array([0.6, 0.85, 0.9])
# beta, theta = 0.5, 0.6
beta = 0.5
thetas = np.array([0.75])
# sigmas = [ 0.01, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5]
# sigmas = np.array([0.01, 0.05, 0.07, 0.09])
times = []
# set distance ERM parameters
distance_multiplier = 1e6
def AddPedestal(plant):
"""
Creates the pedestal.
"""
pedestal_instance = plant.AddModelInstance("pedestal_base")
bodies = []
names = ["left", "right"]
x_positions = [
(PLYWOOD_LENGTH - THICK_PLYWOOD_THICKNESS)/2,
-(PLYWOOD_LENGTH - THICK_PLYWOOD_THICKNESS)/2
]
for name, x_position in zip(names, x_positions):
full_name = "pedestal_" + name + "_body"
body = plant.AddRigidBody(
full_name,
pedestal_instance,
SpatialInertia(mass=1, # Doesn't matter because it's welded
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia.SolidBox(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PEDESTAL_Z_DIM)
))
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PLYWOOD_LENGTH
),
full_name,
pydrake.multibody.plant.CoulombFriction(1, 1)
)
plant.RegisterVisualGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
THICK_PLYWOOD_THICKNESS,
PEDESTAL_Y_DIM,
PLYWOOD_LENGTH
),
full_name,
[0.4, 0.4, 0.4, 1]) # RGBA color
plant.WeldFrames(
plant.world_frame(),
plant.GetFrameByName(full_name, pedestal_instance),
RigidTransform(RotationMatrix(), [
x_position,
0,
PLYWOOD_LENGTH/2+ bump_z
]
))
# Add bump at the bottom
full_name = "pedestal_bottom_body"
body = plant.AddRigidBody(
full_name,
pedestal_instance,
SpatialInertia(mass=1, # Doesn't matter because it's welded
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia.SolidBox(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z)
))
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z
),
full_name,
pydrake.multibody.plant.CoulombFriction(1, 1)
)
plant.RegisterVisualGeometry(
body,
RigidTransform(),
pydrake.geometry.Box(
PEDESTAL_X_DIM,
PEDESTAL_Y_DIM,
bump_z
),
full_name,
[0.4, 0.4, 0.4, 1]) # RGBA color
plant.WeldFrames(
plant.world_frame(),
plant.GetFrameByName(full_name, pedestal_instance),
RigidTransform(RotationMatrix(), [
0,
0,
bump_z/2
]
))
return pedestal_instance
def torch_tf_to_drake_tf(tf):
return RigidTransform(tf.cpu().detach().numpy())
def run_benchmark(
zmq_url=None,
P_WORLD_TARGET=np.array([-1, 1, 0]),
MAX_APPROACH_ANGLE=-45 / 180.0 * np.pi,
OBJECT_TO_TOSS="ball",
GRIPPER_TO_OBJECT_COM_DIST=0.11,
LAUNCH_ANGLE_THRESH=3 / 180.0 * np.pi, # 3 seems to work well?
verbose=False,
**kwargs,
):
# Initialize global plants ONCE for IK calculations
# This speed up exectuation greatly.
GLOBAL_PLANT, GLOBAL_CONTEXT = get_basic_manip_station()
"""
https://schunk.com/fileadmin/pim/docs/IM0026091.PDF - gripper frame to tip is about .115 m for reference
GRIPPER_TO_OBJECT_FRAME_DIST = 0.12 # meters, this is how much "above" the balls origin we must send the gripper body frame in order to successfully grasp the object
OBJECT_FRAME_TO_COM_DIST = 0.05 / 2
"""
T_world_target = RigidTransform(RotationMatrix(), P_WORLD_TARGET)
T_world_objectInitial = RigidTransform(
#p=[-.1, -.69, 1.04998503e-01], # sphere
p=[-0.1, -0.69, 0.09], # foam_brick
R=RotationMatrix.MakeZRotation(np.pi / 2.0))
T_world_gripperObject = RigidTransform(
p=T_world_objectInitial.translation() +
np.array([0, 0, 0.025 + GRIPPER_TO_OBJECT_COM_DIST]),
R=RotationMatrix.MakeXRotation(-np.pi / 2.0))
T_world_objectCOM = RigidTransform(
T_world_objectInitial.rotation(),
T_world_objectInitial.translation() + np.array([0, 0, 0.025]))
# Set starting and ending joint angles for throw
throw_heading = np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0])
ja1 = throw_heading - np.pi
# TODO: edit these to work better for large angles.
PRETHROW_JA = np.array([ja1, 0, 0, 1.9, 0, -1.9, 0, 0, 0])
THROWEND_JA = np.array([ja1, 0, 0, 0.4, 0, -0.4, 0, 0, 0])
T_world_prethrowPose = jointangles_to_pose(
plant=GLOBAL_PLANT,
context=GLOBAL_CONTEXT,
jointangles=PRETHROW_JA[:7],
)
T_world_robotInitial, meshcat = setup_manipulation_station(
T_world_objectInitial,
zmq_url,
T_world_target,
manipuland=OBJECT_TO_TOSS)
#object frame viz
if meshcat:
visualize_transform(meshcat, "T_world_obj0", T_world_objectInitial)
visualize_transform(meshcat, "T_world_objectCOM", T_world_objectCOM)
visualize_transform(meshcat, "T_world_gripperObject",
T_world_gripperObject)
T_world_target = RigidTransform(p=P_WORLD_TARGET, R=RotationMatrix())
visualize_transform(meshcat, "target", T_world_target)
def throw_objective(inp, g=9.81, alpha=1, return_other=None):
throw_motion_time, release_frac = inp
release_ja = PRETHROW_JA + release_frac * (THROWEND_JA - PRETHROW_JA)
T_world_releasePose = jointangles_to_pose(plant=GLOBAL_PLANT,
context=GLOBAL_CONTEXT,
jointangles=release_ja[:7])
p_release = (T_world_releasePose.translation() +
T_world_releasePose.rotation().multiply(
[0, GRIPPER_TO_OBJECT_COM_DIST, 0]))
J_release = spatial_velocity_jacobian_at_jointangles(
plant=GLOBAL_PLANT,
context=GLOBAL_CONTEXT,
jointangles=release_ja[:7],
gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST # <==== important
)[3:6]
v_release = J_release @ (
(THROWEND_JA - PRETHROW_JA) / throw_motion_time)[:7]
if v_release[:2] @ (P_WORLD_TARGET - p_release)[:2] <= 0:
return 1000
x = np.linalg.norm((P_WORLD_TARGET - p_release)[:2])
y = (P_WORLD_TARGET - p_release)[2]
vx = np.linalg.norm(v_release[:2])
vy = v_release[2]
tta = x / vx
y_hat = vy * tta - 0.5 * g * tta**2
phi_hat = np.arctan((vy - g * tta) / vx)
objective = ((y_hat - y)**2 +
np.maximum(phi_hat - MAX_APPROACH_ANGLE, 0)**2
#+ (phi_hat - MAX_APPROACH_ANGLE) ** 2
)
if objective < 1e-6:
# if we hit target at correct angle
# then try to move as slow as possible
objective -= throw_motion_time**2
if return_other == "launch":
return np.arctan2(vy, vx)
elif return_other == "land":
return phi_hat
elif return_other == "tta":
return tta
else:
return objective
res = scipy.optimize.differential_evolution(throw_objective,
bounds=[(1e-3, 3), (0.1, 0.9)],
seed=43)
throw_motion_time, release_frac = res.x
assert throw_motion_time > 0
assert 0 < release_frac < 1
plan_launch_angle = throw_objective(res.x, return_other="launch")
plan_land_angle = throw_objective(res.x, return_other="land")
tta = throw_objective(res.x, return_other="tta")
if verbose:
print(f"Throw motion will take: {throw_motion_time:.4f} seconds")
print(f"Releasing at {release_frac:.4f} along the motion")
print(f"Launch angle (degrees): {plan_launch_angle / np.pi * 180.0}")
print(f"Plan land angle (degrees): {plan_land_angle / np.pi * 180.0}")
print(f"time to arrival: {tta}")
'''
timings
'''
t_goToObj = 1.0
t_holdObj = 2.0
t_goToPreobj = 1.0
t_goToWaypoint = 2.0
t_goToPrethrow = 4.0 # must be greater than 1.0 for a 1 second hold to stabilize
t_goToThrowEnd = throw_motion_time
# plan pickup
t_lst, q_knots, total_time = plan_pickup(T_world_robotInitial,
T_world_gripperObject,
t_goToObj=t_goToObj,
t_holdObj=t_holdObj,
t_goToPreobj=t_goToPreobj)
# clear the bins via a waypoint
T_world_hackyWayPoint = RigidTransform(
p=[-.6, -0.0, 0.6],
R=RotationMatrix.MakeXRotation(
-np.pi / 2.0
), #R_WORLD_PRETHROW, #RotationMatrix.MakeXRotation(-np.pi/2.0),
)
t_lst, q_knots = add_go_to_pose_via_jointinterpolation(
T_world_robotInitial,
T_world_hackyWayPoint,
t_start=total_time,
t_lst=t_lst,
q_knots=q_knots,
time_interval_s=t_goToWaypoint)
# go to prethrow
t_lst, q_knots = add_go_to_ja_via_jointinterpolation(
pose_to_jointangles(T_world_hackyWayPoint),
PRETHROW_JA,
t_start=total_time + t_goToWaypoint,
t_lst=t_lst,
q_knots=q_knots,
time_interval_s=t_goToPrethrow,
hold_time_s=1.0,
)
# go to throw
t_lst, q_knots = add_go_to_ja_via_jointinterpolation(
PRETHROW_JA,
THROWEND_JA,
t_start=total_time + t_goToWaypoint + t_goToPrethrow,
t_lst=t_lst,
q_knots=q_knots,
time_interval_s=t_goToThrowEnd,
num_samples=30,
include_end=True)
# turn trajectory into joint space
q_knots = np.array(q_knots)
q_traj = PiecewisePolynomial.CubicShapePreserving(t_lst, q_knots[:, 0:7].T)
'''
Gripper reference trajectory (not used, we use a closed loop controller instead)
'''
# make gripper trajectory
assert t_holdObj > 1.5
gripper_times_lst = np.array([
0.,
t_goToObj,
1.0, # pickup object
0.25, # pickup object
t_holdObj - 1.25, # pickup object
t_goToPreobj,
t_goToWaypoint,
t_goToPrethrow,
release_frac * t_goToThrowEnd,
1e-9,
(1 - release_frac) * t_goToThrowEnd,
])
gripper_cumulative_times_lst = np.cumsum(gripper_times_lst)
GRIPPER_OPEN = 0.5
GRIPPER_CLOSED = 0.0
gripper_knots = np.array([
GRIPPER_OPEN,
GRIPPER_OPEN,
GRIPPER_OPEN, # pickup object
GRIPPER_CLOSED, # pickup object
GRIPPER_CLOSED, # pickup object
GRIPPER_CLOSED,
GRIPPER_CLOSED,
GRIPPER_CLOSED,
GRIPPER_CLOSED,
GRIPPER_OPEN,
GRIPPER_OPEN,
]).reshape(1, gripper_times_lst.shape[0])
g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst,
gripper_knots)
get_gripper_controller_3 = lambda station_plant: GripperControllerUsingIiwaStateV3(
plant=station_plant,
gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST,
T_world_objectPickup=T_world_gripperObject,
T_world_prethrow=T_world_prethrowPose,
planned_launch_angle=plan_launch_angle,
launch_angle_thresh=LAUNCH_ANGLE_THRESH,
dbg_state_prints=verbose)
# do the thing
simulator, _, meshcat, loggers = BuildAndSimulateTrajectory(
q_traj=q_traj,
g_traj=g_traj,
get_gripper_controller=get_gripper_controller_3,
T_world_objectInitial=
T_world_objectInitial, # where to init the object in the world
T_world_targetBin=
T_world_target, # where the ball should hit - aka where the bin will catch it
zmq_url=zmq_url,
time_step=
1e-3, # target (-6, 6, -1). 1e-3 => overshoot barely, 1e-4 => undershoot barely, look around 7.92-7.94 s in sim
include_target_bin=False,
manipuland=OBJECT_TO_TOSS)
fly_time = max(tta + 1, 2)
if verbose:
print(
f"Throw motion should happen from 9.5 seconds to {10 + throw_motion_time} seconds"
)
print(f"Running for {q_traj.end_time() + fly_time} seconds")
if meshcat:
meshcat.start_recording()
simulator.AdvanceTo(q_traj.end_time() + fly_time)
if meshcat:
meshcat.stop_recording()
meshcat.publish_recording()
all_log_times = loggers["state"].sample_times()
chosen_idxs = (9 < all_log_times) & (all_log_times < 100)
log_times = loggers["state"].sample_times()[chosen_idxs]
log_states = loggers["state"].data()[:, chosen_idxs]
p_objects = np.zeros((len(log_times), 3))
p_objects[:, 0] = log_states[4]
p_objects[:, 1] = log_states[5]
p_objects[:, 2] = log_states[6]
deltas = p_objects - P_WORLD_TARGET
land_idx = np.where(deltas[:, 2] > 0)[0][-1]
p_land = p_objects[land_idx]
is_overthrow = np.linalg.norm(p_land[:2]) > np.linalg.norm(
P_WORLD_TARGET[:2])
delta_land = deltas[land_idx]
land_pos_error = np.linalg.norm(delta_land) * (1 if is_overthrow else -1)
aim_angle_error = (np.arctan2(p_land[1], p_land[0]) -
np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0]))
v_land = ((p_objects[land_idx] - p_objects[land_idx - 1]) /
(log_times[land_idx] - log_times[land_idx - 1]))
sim_land_angle = np.arctan(v_land[2] / np.linalg.norm(v_land[:2]))
land_angle_error = sim_land_angle - plan_land_angle
ret_dict = dict(
land_pos_error=land_pos_error,
land_angle_error=land_angle_error,
aim_angle_error=aim_angle_error,
time_to_arrival=tta,
land_time=log_times[land_idx],
land_x=p_land[0],
land_y=p_land[1],
land_z=p_land[2],
sim_land_angle=sim_land_angle,
plan_land_angle=plan_land_angle,
plan_launch_angle=plan_launch_angle,
release_frac=release_frac,
throw_motion_time=throw_motion_time,
)
return ret_dict
def AddRgbdSensors(builder,
plant,
scene_graph,
also_add_point_clouds=True,
model_instance_prefix="camera",
properties=None,
renderer=None):
"""
Adds a RgbdSensor to every body in the plant with a name starting with
body_prefix. If camera_info is None, then a default camera info will be
used. If renderer is None, then we will assume the name 'my_renderer', and
create a VTK renderer if a renderer of that name doesn't exist.
"""
if not renderer:
renderer = "my_renderer"
if not scene_graph.HasRenderer(renderer):
scene_graph.AddRenderer(renderer,
MakeRenderEngineVtk(RenderEngineVtkParams()))
if not properties:
properties = DepthCameraProperties(width=640,
height=480,
fov_y=np.pi / 4.0,
renderer_name=renderer,
z_near=0.1,
z_far=10.0)
for index in range(plant.num_model_instances()):
model_instance_index = ModelInstanceIndex(index)
model_name = plant.GetModelInstanceName(model_instance_index)
if model_name.startswith(model_instance_prefix):
body_index = plant.GetBodyIndices(model_instance_index)[0]
rgbd = builder.AddSystem(
RgbdSensor(parent_id=plant.GetBodyFrameIdOrThrow(body_index),
X_PB=RigidTransform(),
properties=properties,
show_window=False))
rgbd.set_name(model_name)
builder.Connect(scene_graph.get_query_output_port(),
rgbd.query_object_input_port())
# Export the camera outputs
builder.ExportOutput(rgbd.color_image_output_port(),
f"{model_name}_rgb_image")
builder.ExportOutput(rgbd.depth_image_32F_output_port(),
f"{model_name}_depth_image")
builder.ExportOutput(rgbd.label_image_output_port(),
f"{model_name}_label_image")
if also_add_point_clouds:
# Add a system to convert the camera output into a point cloud
to_point_cloud = builder.AddSystem(
DepthImageToPointCloud(
camera_info=rgbd.depth_camera_info(),
fields=BaseField.kXYZs
| BaseField.kRGBs))
builder.Connect(rgbd.depth_image_32F_output_port(),
to_point_cloud.depth_image_input_port())
builder.Connect(rgbd.color_image_output_port(),
to_point_cloud.color_image_input_port())
class ExtractBodyPose(LeafSystem):
def __init__(self, body_index):
LeafSystem.__init__(self)
self.body_index = body_index
self.DeclareAbstractInputPort(
"poses",
plant.get_body_poses_output_port().Allocate())
self.DeclareAbstractOutputPort(
"pose",
lambda: AbstractValue.Make(RigidTransform()),
self.CalcOutput)
def CalcOutput(self, context, output):
poses = self.EvalAbstractInput(context, 0).get_value()
pose = poses[int(self.body_index)]
output.get_mutable_value().set(pose.rotation(),
pose.translation())
camera_pose = builder.AddSystem(ExtractBodyPose(body_index))
builder.Connect(plant.get_body_poses_output_port(),
camera_pose.get_input_port())
builder.Connect(camera_pose.get_output_port(),
to_point_cloud.GetInputPort("camera_pose"))
# Export the point cloud output.
builder.ExportOutput(to_point_cloud.point_cloud_output_port(),
f"{model_name}_point_cloud")
times["pick_end"] = times["pick_start"] + 2.0
times["postpick"] = times["pick_end"] + 2.0
time_to_from_clearance = 10.0 * \
np.linalg.norm(X_GprepickGclearance.translation())
times["clearance"] = times["postpick"] + time_to_from_clearance
times["preplace"] = times["clearance"] + time_to_from_clearance
times["place_start"] = times["preplace"] + 2.0
times["place_end"] = times["place_start"] + 2.0
times["postplace"] = times["place_end"] + 2.0
return X_G, times
X_G = {
"initial":
RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2.0),
[0, -0.25, 0.25])
}
X_O = {
"initial":
RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
[-.2, -.75, 0.025]),
"goal":
RigidTransform(RotationMatrix.MakeZRotation(np.pi), [.75, 0, 0.025])
}
X_G, times = make_gripper_frames(X_G, X_O)
print(
f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute."
)
def make_gripper_position_trajectory(X_G, times):
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()
# Create a direct collocation problem
prog = DirectCollocation(
plant,
context0,
num_time_samples=21,
maximum_timestep=0.20,
minimum_timestep=0.05,
input_port_index=plant.get_actuation_input_port().get_index())
prog.AddEqualTimeIntervalsConstraints()
# Add initial and final state constraints
def generate_image(image_num):
filename_base = os.path.join(path, f"{image_num:05d}")
builder = pydrake.systems.framework.DiagramBuilder()
plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.0005)
parser = pydrake.multibody.parsing.Parser(plant)
parser.AddModelFromFile(pydrake.common.FindResourceOrThrow(
"drake/examples/manipulation_station/models/bin.sdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("bin_base"))
inspector = scene_graph.model_inspector()
instance_id_to_class_name = dict()
for object_num in range(rng.integers(1,10)):
this_object = ycb[rng.integers(len(ycb))]
class_name = os.path.splitext(this_object)[0]
sdf = pydrake.common.FindResourceOrThrow("drake/manipulation/models/ycb/sdf/" + this_object)
instance = parser.AddModelFromFile(sdf, f"object{object_num}")
frame_id = plant.GetBodyFrameIdOrThrow(
plant.GetBodyIndices(instance)[0])
geometry_ids = inspector.GetGeometries(
frame_id, pydrake.geometry.Role.kPerception)
for geom_id in geometry_ids:
instance_id_to_class_name[int(inspector.GetPerceptionProperties(geom_id).GetProperty("label", "id"))] = class_name
plant.Finalize()
if not debug:
with open(filename_base + ".json", "w") as f:
json.dump(instance_id_to_class_name, f)
renderer = "my_renderer"
scene_graph.AddRenderer(
renderer, pydrake.geometry.render.MakeRenderEngineVtk(pydrake.geometry.render.RenderEngineVtkParams()))
depth_camera = DepthRenderCamera(RenderCameraCore(
renderer, CameraInfo(width=640, height=480, fov_y=np.pi / 4.0),
ClippingRange(near=0.1, far=10.0),
RigidTransform()),
DepthRange(0.1, 10.0))
camera = builder.AddSystem(
pydrake.systems.sensors.RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=RigidTransform(
RollPitchYaw(np.pi, 0, np.pi/2.0),
[0, 0, .8]),
depth_camera=depth_camera,
show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.label_image_output_port(), "label_image")
diagram = builder.Build()
while True:
simulator = pydrake.systems.analysis.Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)
z = 0.1
for body_index in plant.GetFloatingBaseBodies():
tf = RigidTransform(
pydrake.math.UniformlyRandomRotationMatrix(generator),
[rng.uniform(-.15,.15), rng.uniform(-.2, .2), z])
plant.SetFreeBodyPose(plant_context,
plant.get_body(body_index),
tf)
z += 0.1
try:
simulator.AdvanceTo(1.0)
break
except RuntimeError:
# I've chosen an aggressive simulation time step which works most
# of the time, but can fail occasionally.
pass
color_image = diagram.GetOutputPort("color_image").Eval(context)
label_image = diagram.GetOutputPort("label_image").Eval(context)
if debug:
plt.figure()
plt.subplot(121)
plt.imshow(color_image.data)
plt.axis('off')
plt.subplot(122)
plt.imshow(colorize_labels(label_image.data))
plt.axis('off')
plt.show()
else:
Image.fromarray(color_image.data).save(f"{filename_base}.png")
np.save(f"{filename_base}_mask", label_image.data)
def CalcOutput(self, context, output):
# ============================ LOAD INPUTS ============================
# Positions and velocities
rot_vec_M = self.GetInputPort("pose_M_rotational").Eval(context)
p_M = self.GetInputPort("pose_M_translational").Eval(context)
omega_M = self.GetInputPort("vel_M_rotational").Eval(context)
v_M = self.GetInputPort("vel_M_translational").Eval(context)
# Manipulator inputs
M = np.array(self.GetInputPort("M").Eval(context)).reshape(
(self.nq, self.nq))
J = np.array(self.GetInputPort("J").Eval(context)).reshape(
(6, self.nq))
Jdot_qdot = np.expand_dims(
np.array(self.GetInputPort("Jdot_qdot").Eval(context)), 1)
Cv = np.expand_dims(np.array(self.GetInputPort("Cv").Eval(context)), 1)
# Impedance
K = np.diag(self.GetInputPort("K").Eval(context))
D = np.diag(self.GetInputPort("D").Eval(context))
x0 = np.expand_dims(self.GetInputPort("x0").Eval(context), 1)
dx0 = np.expand_dims(self.GetInputPort("dx0").Eval(context), 1)
# ==================== CALCULATE INTERMEDIATE TERMS ===================
# Actual values
d_x = np.expand_dims(np.array(list(omega_M) + list(v_M)), 1)
x = np.expand_dims(np.array(list(rot_vec_M) + list(p_M)), 1)
Mq = M
# TODO: Should this be pinv? (Copying from Sangbae's notes)
Mx = np.linalg.inv(np.matmul(np.matmul(J, np.linalg.inv(Mq)), J.T))
# ===================== CALCULATE CONTROL OUTPUTS =====================
# print(np.matmul(np.linalg.inv(Mq), Cv).shape)
Vq = Cv
compliance_terms = np.matmul(D, dx0 - d_x) \
+ \
np.matmul(K, x0 - x)
cancelation_terms = np.matmul(
Mx,
np.matmul(
np.matmul(J, np.linalg.inv(Mq)),
Vq
) \
- \
Jdot_qdot
)
F_ctrl = cancelation_terms + compliance_terms
tau_ctrl = np.matmul(J.T, F_ctrl)
# ======================== UPDATE DEBUG VALUES ========================
self.debug["dx0"].append(dx0)
self.debug["x0"].append(x0)
self.debug["times"].append(context.get_time())
# ======================== UPDATE VISUALIZATION =======================
visualization.AddMeshcatTriad(
self.meshcat,
"impedance_setpoint",
X_PT=RigidTransform(p=x0[3:].flatten(),
R=RotationMatrix(RollPitchYaw(x0[:3]))))
visualization.AddMeshcatTriad(
self.meshcat,
"manipulator_pose",
X_PT=RigidTransform(p=p_M.flatten(),
R=RotationMatrix(RollPitchYaw(rot_vec_M))))
output.SetFromVector(tau_ctrl.flatten())
def __init__(self, tf):
geom = PhysicsGeometryInfo(fixed=False)
geom.register_model_file(
drake_tf_to_torch_tf(RigidTransform(p=[0.0, 0., 0.0])),
"models/misc/chopstick/model.sdf")
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
xyz_rules = [
SamePositionRule(),
WorldFrameBBoxRule.from_bounds(lb=torch.tensor([0.0, 0.2, 0.3]),
ub=torch.ones(3) * 3.),
WorldFrameBBoxOffsetRule.from_bounds(lb=torch.tensor([0.0, 0.2, 0.3]),
ub=torch.ones(3) * 5.),
WorldFrameGaussianOffsetRule(mean=torch.tensor([0.0, 0.2, 0.3]),
variance=torch.ones(3)),
ParentFrameGaussianOffsetRule(mean=torch.tensor([0.0, 0.2, 0.3]),
variance=torch.ones(3)),
WorldFramePlanarGaussianOffsetRule(mean=torch.tensor([0.0, 0.2]),
variance=torch.ones(2),
plane_transform=RigidTransform(
p=np.array([1., 2., 3.]),
rpy=RollPitchYaw(1., 2., 3.)))
]
rotation_rules = [
SameRotationRule(),
UnconstrainedRotationRule(),
UniformBoundedRevoluteJointRule.from_bounds(axis=torch.tensor([0., 1.,
0.]),
lb=-np.pi / 2.,
ub=np.pi / 2.),
WorldFrameBinghamRotationRule(M=torch.eye(4),
Z=torch.tensor([-1., -1., -1., 0.])),
ParentFrameBinghamRotationRule(M=torch.eye(4),
Z=torch.tensor([-1., -1., -1., 0.]))
]
def draw_tree(tree, vis, prefix="", draw_regions=False):
# Given a scene tree (nx.DiGraph), draw it in the
# specified meshcat visualizer.
# Draw the scene geometry flat, to keep TFs easy.
name_prefix = prefix + "scene"
vis[name_prefix].delete()
k = 0
for node in tree.nodes:
name = name_prefix + "/%s_%03d" % (node.__class__.__name__, k)
if node.geometry is not None:
color = node.geometry_color
alpha = 1.0
vis[name].set_object(
node.geometry,
meshcat_geom.MeshLambertMaterial(color=color, opacity=alpha, transparent=(alpha != 1.))
)
tf = node.tf.GetAsMatrix4()
geom_tf = node.geometry_tf.GetAsMatrix4()
tf = tf.dot(geom_tf)
tf[:3, :3] = tf[:3, :3].dot(np.diag(node.geometry_scale))
print(tf)
vis[name].set_transform(tf)
k += 1
# Draw the tree structure.
tree_prefix = prefix + "tree"
vis[tree_prefix].delete()
k = 0
for node in tree.nodes:
name = tree_prefix + "/" + node.__class__.__name__ + "_%03d" % k
k += 1
# Draw node as randomly colored sphere
color = random.randint(0, 0xFFFFFF)
alpha = 0.5
vis[name]["triad"].set_object(
meshcat_geom.triad(scale=0.1)
)
vis[name]["sphere"].set_object(
meshcat_geom.Sphere(0.01),
meshcat_geom.MeshToonMaterial(color=color, opacity=alpha, transparent=(alpha != 1.))
)
vis[name].set_transform(node.tf.GetAsMatrix4())
# Draw children
verts = []
for child in tree.successors(node):
# Draw link to child
verts.append(node.tf.translation()),
verts.append(child.tf.translation())
if len(verts) > 0:
verts = np.vstack(verts).T
# Don't want this as a direct child or it'll inherit the transform
vis[name + "_child_connections"].set_object(
meshcat_geom.Line(meshcat_geom.PointsGeometry(verts),
meshcat_geom.LineBasicMaterial(linewidth=50, color=color)))
if draw_regions:
# Draw the child regions for each child
if isinstance(node, (AndNode, OrNode, RepeatingSetNode)):
for info_k, child_info in enumerate(node.child_infos):
region_name = "child_region_%03d" % info_k
lb = child_info.child_xyz_bounds.xyz_min
ub = child_info.child_xyz_bounds.xyz_max
vis[name][region_name].set_object(
meshcat_geom.Box(ub - lb),
meshcat_geom.MeshToonMaterial(color=0x111111, opacity=0.1, transparent=True)
)
tf = RigidTransform(p=(ub+lb)/2)
vis[name][region_name].set_transform(tf.GetAsMatrix4())
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True):
# Make some random lumpy objects
trimeshes = []
np.random.seed(42)
for k in range(3):
# Make a small random number of triangles and chull it
# to get a lumpy object
mesh = trimesh.creation.random_soup(5)
mesh = trimesh.convex.convex_hull(mesh)
trimeshes.append(mesh)
# Create Drake geometry from those objects by adding a small
# sphere at each vertex
sphere_rad = 0.05
cmap = plt.cm.get_cmap('jet')
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.001))
# Add ground
friction = CoulombFriction(0.9, 0.8)
g = pydrake_geom.Box(100., 100., 0.5)
tf = RigidTransform(p=[0., 0., -0.25])
mbp.RegisterVisualGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
diffuse_color=[1.0, 1.0, 1.0, 1.0])
mbp.RegisterCollisionGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
coulomb_friction=friction)
for i, mesh in enumerate(trimeshes):
inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia)
color = cmap(np.random.random())
if verts_geom:
for j, vert in enumerate(mesh.vertices):
g = pydrake_geom.Sphere(radius=sphere_rad)
tf = RigidTransform(p=vert)
mbp.RegisterVisualGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_color_%d" % (i, j),
diffuse_color=color)
mbp.RegisterCollisionGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_collision_%d" %
(i, j),
coulomb_friction=friction)
# And add mesh itself for vis
path = "/tmp/part_%d.obj" % i
trimesh.exchange.export.export_mesh(mesh, path)
g = pydrake_geom.Convex(path)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base" % i,
diffuse_color=color)
if convex_collision_geom:
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base_col" % i,
coulomb_friction=friction)
mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.]))
mbp.Finalize()
return builder, mbp, scene_graph
def plan_throw(
T_world_robotInitial,
T_world_gripperObject,
p_world_target, # np.array of shape (3,)
gripper_to_object_dist,
throw_height=0.5, # meters
prethrow_height=0.2,
prethrow_radius=0.4,
throw_angle=np.pi / 4.0,
meshcat=None,
throw_speed_adjustment_factor=1.0,
):
"""
only works with the "back portion" of the clutter station until we figure out how to move the bins around
motion moves along an arc from a "pre throw" to a "throw" position
"""
theta = 1.0 * np.arctan2(p_world_target[1], p_world_target[0])
print(f"theta={theta}")
T_world_prethrow = RigidTransform(
p=np.array([
prethrow_radius * np.cos(theta), prethrow_radius * np.sin(theta),
prethrow_height
]),
R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply(
RotationMatrix.MakeYRotation((theta - np.pi / 2))))
throw_radius = throw_height - prethrow_height
T_world_throw = RigidTransform(
p=T_world_prethrow.translation() + np.array([
throw_radius * np.cos(theta), throw_radius * np.sin(theta),
throw_height - prethrow_height
]),
R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply(
RotationMatrix.MakeYRotation((theta - np.pi / 2)).multiply(
RotationMatrix.MakeXRotation(-np.pi / 2))))
if meshcat:
visualize_transform(meshcat, "T_world_prethrow", T_world_prethrow)
visualize_transform(meshcat, "T_world_throw", T_world_throw)
p_world_object_at_launch = interpolatePosesArcMotion(
T_world_prethrow, T_world_throw, t=throw_angle /
(np.pi / 2.)).translation() + np.array([0, 0, -gripper_to_object_dist])
pdot_world_launch = interpolatePosesArcMotion_pdot(T_world_prethrow,
T_world_throw,
t=throw_angle /
(np.pi / 2.))
launch_speed_base = np.linalg.norm(pdot_world_launch)
launch_speed_required = get_launch_speed_required(
theta=throw_angle,
x=np.linalg.norm(p_world_target[:2]) -
np.linalg.norm(p_world_object_at_launch[:2]),
y=p_world_target[2] - p_world_object_at_launch[2])
total_throw_time = launch_speed_base / launch_speed_required / throw_speed_adjustment_factor
print(f"p_world_object_at_launch={p_world_object_at_launch}")
print(f"target={p_world_target}")
print(
f"dx={np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2])}"
)
print(f"dy={p_world_target[2] - p_world_object_at_launch[2]}")
print(f"pdot_world_launch={pdot_world_launch}")
print(f"total_throw_time={total_throw_time}")
T_world_hackyWayPoint = RigidTransform(
p=[-.6, -0.0, 0.6],
R=RotationMatrix.MakeXRotation(
-np.pi / 2.0
), #R_WORLD_PRETHROW, #RotationMatrix.MakeXRotation(-np.pi/2.0),
)
# event timings (not all are relevant to pose and gripper)
# initial pose => prethrow => throw => yays
t_goToObj = 1.0
t_holdObj = 0.5
t_goToPreobj = 1.0
t_goToWaypoint = 1.0
t_goToPrethrow = 1.0
t_goToRelease = total_throw_time * throw_angle / (np.pi / 2.)
t_goToThrowEnd = total_throw_time * (1 - throw_angle / (np.pi / 2.))
t_throwEndHold = 3.0
ts = np.array([
t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow,
t_goToRelease, t_goToThrowEnd, t_throwEndHold
])
cum_pose_ts = np.cumsum(ts)
print(cum_pose_ts)
# Create pose trajectory
t_lst = np.linspace(0, cum_pose_ts[-1], 1000)
pose_lst = []
for t in t_lst:
if t < cum_pose_ts[1]:
pose = interpolatePosesLinear(T_world_robotInitial,
T_world_gripperObject,
min(t / ts[0], 1.0))
elif t < cum_pose_ts[2]:
pose = interpolatePosesLinear(T_world_gripperObject,
T_world_robotInitial,
(t - cum_pose_ts[1]) / ts[2])
elif t < cum_pose_ts[3]:
pose = interpolatePosesLinear(T_world_robotInitial,
T_world_hackyWayPoint,
(t - cum_pose_ts[2]) / ts[3])
elif t <= cum_pose_ts[4]:
pose = interpolatePosesLinear(T_world_hackyWayPoint,
T_world_prethrow,
(t - cum_pose_ts[3]) / ts[4])
else:
pose = interpolatePosesArcMotion(
T_world_prethrow, T_world_throw,
min((t - cum_pose_ts[4]) / (ts[5] + ts[6]), 1.0))
pose_lst.append(pose)
# Create gripper trajectory.
gripper_times_lst = np.array([
0., t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow,
t_goToRelease, t_goToThrowEnd, t_throwEndHold
])
gripper_cumulative_times_lst = np.cumsum(gripper_times_lst)
GRIPPER_OPEN = 0.5
GRIPPER_CLOSED = 0.0
gripper_knots = np.array([
GRIPPER_OPEN, GRIPPER_OPEN, GRIPPER_CLOSED, GRIPPER_CLOSED,
GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_OPEN,
GRIPPER_CLOSED
]).reshape(1, gripper_times_lst.shape[0])
g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst,
gripper_knots)
return t_lst, pose_lst, g_traj
import numpy as np
from systems.visualization import Visualizer
from pydrake.all import PiecewisePolynomial, RigidTransform
from pydrake.math import RigidTransform, RollPitchYaw
from scipy.stats import norm
from scipy.special import erfinv
from systems.timestepping import TimeSteppingMultibodyPlant
from utilities import load
import pickle
_file = "systems/urdf/single_legged_hopper.urdf"
plant = TimeSteppingMultibodyPlant(file=_file)
body_inds = plant.multibody.GetBodyIndices(plant.model_index)
base_frame = plant.multibody.get_body(body_inds[0]).body_frame()
plant.multibody.WeldFrames(plant.multibody.world_frame(), base_frame,
RigidTransform())
plant.Finalize()
def chance_constraint(beta, theta, sigma):
lb = -np.sqrt(2) * sigma * erfinv(2 * beta - 1)
ub = -np.sqrt(2) * sigma * erfinv(1 - 2 * theta)
return lb, ub
def plot(x, u, l, t):
fig1, axs1 = plt.subplots(6, 1)
# plot configurations
axs1[0].set_title('Configuration Trajectory')
axs1[0].plot(t, x[0, :], linewidth=1.5)
def addSphere(plant, scene_graph=None):
"""Adds the manipulator."""
sphere = plant.AddModelInstance("sphere")
# Initialize sphere body
sphere_body = plant.AddRigidBody(
"sphere_body", sphere,
pydrake.multibody.tree.SpatialInertia(
mass=MASS,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=pydrake.multibody.tree.UnitInertia(1.0, 1.0, 1.0)))
# Initialize false bodies
empty_inertia = SpatialInertia(0, [0, 0, 0], UnitInertia(0, 0, 0))
for i in range(5):
# Add false bodies for control joints
false_body = plant.AddRigidBody("false_body{}".format(i), sphere,
empty_inertia)
# plant.WeldFrames(
# plant.world_frame(),
# false_body.body_frame(),
# RigidTransform()
# )
# Register geometry
if plant.geometry_source_is_registered():
col_geom = plant.RegisterCollisionGeometry(
sphere_body, RigidTransform(), pydrake.geometry.Sphere(RADIUS),
"sphere_body", pydrake.multibody.plant.CoulombFriction(1, 1))
plant.RegisterVisualGeometry(sphere_body, RigidTransform(),
pydrake.geometry.Sphere(RADIUS),
"sphere_body", [.9, .5, .5, 1.0]) # Color
# jnt = plant.AddJoint(pydrake.multibody.tree.Joint(
# "sphere_joint",
# plant.world_frame(),
# sphere_body))
# plant.AddJointActuator("sphere_actuator", jnt)
# Linear x control
sphere_x_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_x_translation", plant.world_frame(),
plant.GetFrameByName("false_body0"), [1, 0, 0], -1, 1))
plant.AddJointActuator("sphere_x_translation", sphere_x_translation)
sphere_x_translation.set_default_translation(0)
# Linear y control
sphere_y_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_y_translation", plant.GetFrameByName("false_body0"),
plant.GetFrameByName("false_body1"), [0, 1, 0], -1, 1))
plant.AddJointActuator("sphere_y_translation", sphere_y_translation)
sphere_y_translation.set_default_translation(0.2)
# Linear z control
sphere_z_translation = plant.AddJoint(
pydrake.multibody.tree.PrismaticJoint(
"sphere_z", plant.GetFrameByName("false_body1"),
plant.GetFrameByName("false_body2"), [0, 0, 1], -1, 1))
sphere_z_translation.set_default_translation(0.25)
plant.AddJointActuator("sphere_z", sphere_z_translation)
# Rotational x control
sphere_x_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_x_rotation",
plant.GetFrameByName("false_body2"),
plant.GetFrameByName("false_body3"), [1, 0, 0],
damping=0))
sphere_x_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_x_rotation", sphere_x_rotation)
# Rotational y control
sphere_y_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_y_rotation",
plant.GetFrameByName("false_body3"),
plant.GetFrameByName("false_body4"), [0, 1, 0],
damping=0))
sphere_y_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_y_rotation", sphere_y_rotation)
# Rotational z control
sphere_z_rotation = plant.AddJoint(
pydrake.multibody.tree.RevoluteJoint(
"sphere_z_rotation",
plant.GetFrameByName("false_body4"),
plant.GetFrameByName("sphere_body"), [0, 0, 1],
damping=0))
sphere_z_rotation.set_default_angle(0)
plant.AddJointActuator("sphere_z_rotation", sphere_z_rotation)
return sphere
def generate_random_transform(seed_num):
generator = RandomGenerator(seed_num)
R_BA = UniformlyRandomRotationMatrix(generator)
p_BA = np.random.RandomState(generator()).rand(3)
X_BA = RigidTransform(R_BA, p_BA)
return X_BA
