def test_AddPositionConstraint2(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
X_AbarA = RigidTransform([-0.1, -0.2, -0.3])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameAbar=self.body2_frame, X_AbarA=X_AbarA,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AbarQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
p_AQ = X_AbarA.inverse() @ p_AbarQ
self.assertTrue(np.greater(
p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(
p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all())
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
np.testing.assert_array_equal(self.plant.GetPositions(
self.ik_two_bodies.context()), q_val)
self.assertIs(self.ik_two_bodies.get_mutable_context(),
self.ik_two_bodies.context())
def get_q_object_final(self, q_object_init, ee_init, ee_final):
"""
Input:
q_object_init: [quaternion, xyz]
ee_init: [x y z r p y]
ee_final: [x y z r p y]
xyz_iiwa: [x, y, z]
Return:
q_object_final: [quaternion, xyz]
"""
xyz_obj = np.array(q_object_init[4:]).transpose()
# xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame
quaternion_obj = Quaternion(np.array(q_object_init[:4]))
rot_obj = RotationMatrix(quaternion_obj)
X_WO = RigidTransform(rot_obj, xyz_obj)
xyz_ee_init = np.array(ee_init[:3]).transpose()
rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix()
X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init)
xyz_ee_final = np.array(ee_final[:3]).transpose()
rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix()
X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final)
X_EO = X_WE1.inverse().multiply(X_WO)
X_WO2 = X_WE2.multiply(X_EO)
wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz()
xyz_obj_final = X_WO2.translation()
# xyz_ee_obj_init = xyz_obj - xyz_ee_init
# rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose())
# xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init)
# # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame
# xyz_obj_final = xyz_ee_final + xyz_ee_obj_final
# rot_obj_final = rot_ee_init_final.multiply(rot_obj)
# quaternion_obj_final = rot_obj_final.ToQuaternion()
# wxyz_obj_final = quaternion_obj_final.wxyz()
q_object_final = []
for i in range(4):
q_object_final.append(wxyz_obj_final[i])
for i in range(3):
q_object_final.append(xyz_obj_final[i])
return q_object_final
bin_2 = parser.AddModelFromFile(bin_file, model_name="bin_2")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("bin_base", bin_2), X_AB=RigidTransform(np.array([0.65, 0.5, 0.0])))
scene_graph.AddRenderer("renderer", MakeRenderEngineVtk(RenderEngineVtkParams()))
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Important to do tidying work
plant.Finalize()
# Gripper pose relative to object when in grasp
p_GgraspO = [0, 0, 0.13]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi)
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
# Pregrasp is negative z in the gripper frame
X_GgraspGpregrasp = RigidTransform([0, 0.0, -0.08])
X_O = {"initial": RigidTransform([0.65, 0.5, 0.015]),
"goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0), [0.0, 0.0, 0.015])}
temp_context = plant.CreateDefaultContext()
temp_plant_context = plant.GetMyContextFromRoot(temp_context)
desired_initial_state = np.array([0.0, 0.3, 0.0, -1.3, 0.0, 1.65, 0.9, 0.040, 0.040])
plant.SetPositions(temp_plant_context, panda, desired_initial_state)
X_G = {"initial": plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName("panda_hand"))}
# X_G = {"initial": RigidTransform(RotationMatrix.MakeXRotation(np.pi), [0, -0.25, 0.25])}
class HydraInteractionLeafSystem(LeafSystem):
''' Handles comms with the Hydra, and uses QueryObject inputs from the SceneGraph
to pick closests points when required. Passes this information to the HydraInteractionForceElement
at every update tick.'''
def __init__(self,
mbp,
sg,
all_manipulable_body_ids=[],
zmq_url="default"):
LeafSystem.__init__(self)
self.all_manipulable_body_ids = all_manipulable_body_ids
self.set_name('HydraInteractionLeafSystem')
# Pose bundle (from SceneGraph) input port.
#default_sg_context = sg.CreateDefaultContext()
#print("Default sg context: ", default_sg_context)
#query_object = sg.get_query_output_port().Eval(default_sg_context)
#print("Query object: ", query_object)
#self.DeclareAbstractInputPort("query_object",
# AbstractValue.Make(query_object))
self.pose_bundle_input_port = self.DeclareAbstractInputPort(
"pose_bundle", AbstractValue.Make(PoseBundle(0)))
self.robot_state_input_port = self.DeclareVectorInputPort(
"robot_state",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
self.spatial_forces_output_port = self.DeclareAbstractOutputPort(
"spatial_forces_vector",
lambda: AbstractValue.Make(VectorExternallyAppliedSpatialForced()),
self.DoCalcAbstractOutput)
self.DeclarePeriodicPublish(0.01, 0.0)
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
fwd_pt_in_hydra_frame = RigidTransform(p=[0.0, 0.0, 0.0])
self.vis["hydra_origin"]["hand"].set_object(
meshcat_geom.ObjMeshGeometry.from_file(
os.path.join(os.getcwd(), "hand-regularfinal-scaled-1.obj")))
self.vis["hydra_origin"]["hand"].set_transform(
meshcat_tf.compose_matrix(scale=[0.001, 0.001, 0.001],
angles=[np.pi / 2, 0., np.pi / 2],
translate=[-0.25, 0., 0.]))
#self.vis["hydra_origin"]["center"].set_object(meshcat_geom.Sphere(0.02))
#self.vis["hydra_origin"]["center"].set_transform(meshcat_tf.translation_matrix([-0.025, 0., 0.]))
#self.vis["hydra_origin"]["mid"].set_object(meshcat_geom.Sphere(0.015))
#self.vis["hydra_origin"]["mid"].set_transform(meshcat_tf.translation_matrix([0.0, 0., 0.]))
#self.vis["hydra_origin"]["fwd"].set_object(meshcat_geom.Sphere(0.01))
#self.vis["hydra_origin"]["fwd"].set_transform(fwd_pt_in_hydra_frame.matrix())
#self.vis["hydra_grab"].set_object(meshcat_geom.Sphere(0.01),
# meshcat_geom.MeshLambertMaterial(
# color=0xff22dd,
# alphaMap=0.1))
self.vis["hydra_grab"]["grab_point"].set_object(
meshcat_geom.Sphere(0.01),
meshcat_geom.MeshLambertMaterial(color=0xff22dd, alphaMap=0.1))
# Hide it sketchily
self.vis["hydra_grab"].set_transform(
meshcat_tf.translation_matrix([0., 0., -1000.]))
# State for selecting objects
self.grab_needs_update = False
self.grab_in_progress = False
self.grab_update_hydra_pose = None
self.selected_body = None
self.selected_pose_in_body_frame = None
self.desired_pose_in_world_frame = None
self.stop = False
self.freeze_rotation = False
self.previously_freezing_rotation = False
# Set up subscription to Razer Hydra
self.mbp = mbp
self.mbp_context = mbp.CreateDefaultContext()
self.sg = sg
self.hasNewMessage = False
self.lastMsg = None
self.hydra_origin = RigidTransform(p=[1.0, 0., -0.1],
rpy=RollPitchYaw([0., 0., 0.]))
self.hydra_prescale = 3.0
self.callback_lock = Lock()
self.hydraSubscriber = rospy.Subscriber("/hydra_calib",
razer_hydra.msg.Hydra,
self.callback,
queue_size=1)
print("Waiting for hydra startup...")
while not self.hasNewMessage and not rospy.is_shutdown():
rospy.sleep(0.01)
print("Got hydra.")
def DoCalcAbstractOutput(self, context, y_data):
self.callback_lock.acquire()
if self.selected_body and self.grab_in_progress:
# Simple inverse dynamics PD rule to drive object to desired pose.
body = self.selected_body
# Load in robot state
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
TF_object = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
xyz = TF_object.translation()
R = TF_object.rotation().matrix()
TFd_object = self.mbp.EvalBodySpatialVelocityInWorld(
self.mbp_context, body)
xyzd = TFd_object.translational()
Rd = TFd_object.rotational()
# Match the object position directly to the hydra position.
xyz_desired = self.desired_pose_in_world_frame.translation()
# Regress xyz back to just the hydra pose in the attraction case
if self.previously_freezing_rotation != self.freeze_rotation:
self.selected_body_init_offset = TF_object
self.grab_update_hydra_pose = RigidTransform(
self.desired_pose_in_world_frame)
self.previously_freezing_rotation = self.freeze_rotation
if self.freeze_rotation:
R_desired = self.selected_body_init_offset.rotation().matrix()
else:
# Figure out the relative rotation of the hydra from its initial posture
to_init_hydra_tf = self.grab_update_hydra_pose.inverse()
desired_delta_rotation = to_init_hydra_tf.multiply(
self.desired_pose_in_world_frame).matrix()[:3, :3]
# Transform the current object rotation into the init hydra frame, apply that relative tf, and
# then transform back
to_init_hydra_tf_rot = to_init_hydra_tf.matrix()[:3, :3]
R_desired = to_init_hydra_tf_rot.T.dot(
desired_delta_rotation.dot(
to_init_hydra_tf_rot.dot(
self.selected_body_init_offset.rotation().matrix())
))
# Could also pull the rotation back, but it's kind of nice to be able to recenter the object
# without messing up a randomized rotation.
#R_desired = (self.desired_pose_in_world_frame.rotation().matrix()*self.attract_factor +
# R_desired*(1.-self.attract_factor))
# Apply PD in cartesian space
xyz_e = xyz_desired - xyz
xyzd_e = -xyzd
f = 100. * xyz_e + 10. * xyzd_e
R_err_in_body_frame = np.linalg.inv(R).dot(R_desired)
aa = AngleAxis(R_err_in_body_frame)
tau_p = R.dot(aa.axis() * aa.angle())
tau_d = -Rd
tau = tau_p + 0.1 * tau_d
exerted_force = SpatialForce(tau=tau, f=f)
out = ExternallyAppliedSpatialForce()
out.F_Bq_W = exerted_force
out.body_index = self.selected_body.index()
y_data.set_value(VectorExternallyAppliedSpatialForced([out]))
else:
y_data.set_value(VectorExternallyAppliedSpatialForced([]))
self.callback_lock.release()
def DoPublish(self, context, event):
# TODO(russt): Copied from meshcat_visualizer.py.
# Change this to declare a periodic event with a
# callback instead of overriding DoPublish, pending #9992.
LeafSystem.DoPublish(self, context, event)
self.callback_lock.acquire()
if self.stop:
self.stop = False
if context.get_time() > 0.5:
self.callback_lock.release()
raise StopIteration
#query_object = self.EvalAbstractInput(context, 0).get_value()
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
if self.grab_needs_update:
hydra_tf = self.grab_update_hydra_pose
self.grab_needs_update = False
# If grab point is colliding...
#print [x.distance for x in query_object.ComputeSignedDistanceToPoint(hydra_tf.matrix()[:3, 3])]
# Find closest body to current pose
grab_center = hydra_tf.matrix()[:3, 3]
closest_distance = np.inf
closest_body = self.mbp.get_body(BodyIndex(2))
for body_id in self.all_manipulable_body_ids:
body = self.mbp.get_body(body_id)
offset = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
dist = np.linalg.norm(grab_center - offset.translation())
if dist < closest_distance:
closest_distance = dist
closest_body = body
self.selected_body = closest_body
self.selected_body_init_offset = self.mbp.EvalBodyPoseInWorld(
self.mbp_context, self.selected_body)
self.callback_lock.release()
def callback(self, msg):
''' Control mapping:
Buttons: [Digital trigger, 1, 2, 3, 4, start, joy click]
Digital trigger: buttons[0]
Analog trigger: trigger
Joy: +x is right, +y is fwd
'''
self.callback_lock.acquire()
self.lastMsg = msg
self.hasNewMessage = True
pad_info = msg.paddles[0]
hydra_tf_uncalib = ros_tf_to_rigid_transform(pad_info.transform)
hydra_tf_uncalib.set_translation(hydra_tf_uncalib.translation() *
self.hydra_prescale)
hydra_tf = self.hydra_origin.multiply(hydra_tf_uncalib)
self.desired_pose_in_world_frame = hydra_tf
self.vis["hydra_origin"].set_transform(hydra_tf.matrix())
# Interpret various buttons for changing to scaling
if pad_info.buttons[0] and not self.grab_in_progress:
print("Grabbing")
self.grab_update_hydra_pose = hydra_tf
self.grab_needs_update = True
self.grab_in_progress = True
elif self.grab_in_progress and not pad_info.buttons[0]:
self.grab_in_progress = False
self.selected_body = None
self.freeze_rotation = pad_info.trigger > 0.15
if pad_info.buttons[5]:
self.stop = True
# Optional: use buttons to adjust hydra-reality scaling.
# Disabling for easier onboarding of new users...
#if pad_info.buttons[1]:
# # Scale down
# self.hydra_prescale = max(0.01, self.hydra_prescale * 0.98)
# print("Updated scaling to ", self.hydra_prescale)
#if pad_info.buttons[3]:
# # Scale up
# self.hydra_prescale = min(10.0, self.hydra_prescale * 1.02)
# print("Updated scaling to ", self.hydra_prescale)
#if pad_info.buttons[2]:
# # Translate down
# translation = self.hydra_origin.translation().copy()
# translation[2] -= 0.01
# print("Updated translation to ", translation)
# self.hydra_origin.set_translation(translation)
#if pad_info.buttons[4]:
# # Translate up
# translation = self.hydra_origin.translation().copy()
# translation[2] += 0.01
# print("Updated translation to ", translation)
# self.hydra_origin.set_translation(translation)
#if abs(pad_info.joy[0]) > 0.01 or abs(pad_info.joy[1]) > 0.01:
# # Translate up
# translation = self.hydra_origin.translation().copy()
# translation[1] -= pad_info.joy[0]*0.01
# translation[0] += pad_info.joy[1]*0.01
# print("Updated translation to ", translation)
#self.hydra_origin.set_translation(translation)
self.callback_lock.release()
