def AddPlanarBinAndSimpleBox(plant,
mass=1.0,
mu=1.0,
width=0.2,
depth=0.05,
height=0.3):
parser = Parser(plant)
bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf"))
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("bin_base", bin),
RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
[0, 0, -0.015]))
planar_joint_frame = plant.AddFrame(
FixedOffsetFrame(
"planar_joint_frame", plant.world_frame(),
RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2))))
# TODO(russt): make this a *random* box?
# TODO(russt): move random box to a shared py file.
box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu)
box_joint = plant.AddJoint(
PlanarJoint("box", planar_joint_frame,
plant.GetFrameByName("box", box_instance)))
box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi])
box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2])
box_joint.set_default_translation([0, depth / 2.0])
return box_instance
def make_gripper_frames(X_G, X_O):
"""
Takes a partial specification with X_G["initial"] and X_O["initial"] and X_0["goal"], and
returns a X_G and times with all of the pick and place frames populated.
"""
# Define (again) the gripper pose relative to the object when in grasp.
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
RotationMatrix.MakeZRotation(np.pi / 2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
# pregrasp is negative y in the gripper frame (see the figure!).
X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])
X_G["pick"] = X_O["initial"].multiply(X_OGgrasp)
X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp)
X_G["place"] = X_O["goal"].multiply(X_OGgrasp)
X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp)
# I'll interpolate to a halfway orientation by converting to axis angle and halving the angle
X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"])
angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()
X_GprepickGclearance = RigidTransform(
AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),
X_GprepickGpreplace.translation() / 2.0 + np.array([0, -0.3, 0]))
X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance)
# Not lets set timings
times = {"initial": 0.0}
X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"])
times["prepick"] = times["initial"] + 10.0 * \
np.linalg.norm(X_GinitialGprepick.translation())
# Allow some time for the gripper to close.
times["pick_start"] = times["prepick"] + 2.0
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
def test_X_WB(self):
"""Testing X_WB"""
f = self.notebook_locals['compute_X_WB']
# construct a test case
theta1, theta2, theta3 = np.pi / 3.0, np.pi / 6.0, np.pi / 4.0
R_WA = RotationMatrix.MakeXRotation(theta1)
R_AB = RotationMatrix.MakeZRotation(theta2)
R_CB = RotationMatrix.MakeYRotation(theta3)
X_WA = RigidTransform(R_WA, [0.1, 0.2, 0.5])
X_AB = RigidTransform(R_AB, [0.3, 0.4, 0.1])
X_CB = RigidTransform(R_CB, [0.5, 0.9, 0.7])
test_X_WB = f(X_WA, X_AB, X_CB)
true_X_WB = X_WA.multiply(X_AB)
test_result = test_X_WB.multiply(true_X_WB.inverse())
test_result = test_result.GetAsMatrix4()
self.assertTrue(np.allclose(test_result, np.eye(4)))
def grasp_poses_example():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
grasp = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"
), "grasp")
brick = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
meshcat = ConnectMeshcatVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
B_O = plant.GetBodyByName("base_link", brick)
X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)
B_Ggrasp = plant.GetBodyByName("body", grasp)
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
RotationMatrix.MakeZRotation(np.pi / 2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
X_WGgrasp = X_WO.multiply(X_OGgrasp)
plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
# Open the fingers, too
plant.GetJointByName("left_finger_sliding_joint",
grasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint",
grasp).set_translation(plant_context, 0.054)
meshcat.load()
diagram.Publish(context)
def addPrimitivesEndEffector(plant, scene_graph, sphere_body, arm_instance):
end_effector_RT = RigidTransform(
p=[0, -fraction_of_lower_hemisphere * RADIUS, 0])
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
sphere_body, end_effector_RT, pydrake.geometry.Sphere(RADIUS),
"sphere_body", pydrake.multibody.plant.CoulombFriction(1, 1))
plant.RegisterVisualGeometry(sphere_body, end_effector_RT,
pydrake.geometry.Sphere(RADIUS),
"sphere_body", [.9, .5, .5, 1.0]) # Color
if USE_BOX:
partial_radius = RADIUS * (1 - fraction_of_lower_hemisphere**2)**0.5
box_side = partial_radius * 2
box_height = RADIUS * (1 - fraction_of_lower_hemisphere)
box = pydrake.geometry.Box(box_side, box_side, box_height)
box_body = plant.AddRigidBody(
"box_body", arm_instance,
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)))
if plant.geometry_source_is_registered():
plant.RegisterCollisionGeometry(
box_body, RigidTransform(), box, "box_body",
pydrake.multibody.plant.CoulombFriction(1, 1))
plant.RegisterVisualGeometry(box_body, RigidTransform(), box,
"box_body",
[.9, .5, .5, 1.0]) # Color
geometries = plant.CollectRegisteredGeometries(
[box_body, sphere_body,
plant.GetBodyByName("panda_link8")])
scene_graph.collision_filter_manager().Apply(
CollisionFilterDeclaration().ExcludeWithin(geometries))
plant.WeldFrames(
plant.GetFrameByName("panda_link8", arm_instance),
plant.GetFrameByName("box_body", arm_instance),
RigidTransform(R=RotationMatrix.MakeZRotation(np.pi / 4),
p=[0, 0, 0.065 - box_height / 2]))
def addArm(plant, scene_graph=None):
"""
Creates the panda arm.
"""
parser = pydrake.multibody.parsing.Parser(plant, scene_graph)
arm_instance = parser.AddModelFromFile("models/panda_arm.urdf")
panda_offset = 0
if config.num_links == config.NumLinks.TWO:
panda_offset = IN_TO_M * 22
elif config.num_links == config.NumLinks.FOUR:
panda_offset = IN_TO_M * 40
# Weld panda to world
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("panda_link0", arm_instance),
RigidTransform(RotationMatrix().MakeZRotation(np.pi),
[0, panda_offset, 0]))
sphere_body = plant.AddRigidBody(
"sphere_body", arm_instance,
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)))
if USE_MESH:
addMeshEndEffector(plant, scene_graph, sphere_body)
else:
addPrimitivesEndEffector(plant, scene_graph, sphere_body, arm_instance)
X_P_S = RigidTransform(
RotationMatrix.MakeZRotation(np.pi / 4).multiply(
RotationMatrix.MakeXRotation(-np.pi / 2)),
[0, 0, 0.065]) # Roughly aligns axes with world axes
plant.WeldFrames(plant.GetFrameByName("panda_link8", arm_instance),
plant.GetFrameByName("sphere_body", arm_instance), X_P_S)
return arm_instance
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
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):
"""
Constructs a gripper position trajectory from the plan "sketch".
"""
# The syntax here is a little ugly; we need to pass in a matrix with the samples in the columns.
