def test_inverse_tf(set_seed):
# Test against drake
t = np.random.uniform(-1, 1, size=3)
R = UniformlyRandomRotationMatrix(set_seed)
drake_tf = RigidTransform(p=t, R=R)
tf = drake_tf_to_torch_tf(drake_tf)
tf_inv = invert_torch_tf(tf)
assert torch.allclose(tf_inv, drake_tf_to_torch_tf(drake_tf.inverse()))
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 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 test_grasp_pose(self):
"""Testing grasp pose"""
f = self.notebook_locals['design_grasp_pose']
X_WO = self.notebook_locals['X_WO']
test_X_OG, test_X_WG = f(X_WO)
R_OG = RotationMatrix(np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]).T)
p_OG = [-0.02, 0, 0]
X_OG = RigidTransform(R_OG, p_OG)
X_WG = X_WO.multiply(X_OG)
test_X_CW = f(X_WO)
test_result = test_X_OG.multiply(X_OG.inverse())
test_result = test_result.GetAsMatrix4()
self.assertTrue(np.allclose(test_result, np.eye(4)))
test_result = test_X_WG.multiply(X_WG.inverse())
test_result = test_result.GetAsMatrix4()
self.assertTrue(np.allclose(test_result, np.eye(4)))
def test_X_CW(self):
"""Testing X_CW"""
f = self.notebook_locals['compute_X_CW']
# 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])
true_X_WC = X_WA.multiply(X_AB).multiply(X_CB.inverse())
true_X_CW = true_X_WC.inverse()
test_X_CW = f(X_WA, X_AB, X_CB)
test_result = true_X_CW.multiply(test_X_CW.inverse())
test_result = test_result.GetAsMatrix4()
self.assertTrue(np.allclose(test_result, np.eye(4)))
