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
def buildViewPatches(self, use_random_colors):
''' Generates view patches. self.viewPatches stores a list of
viewPatches for each body (starting at body id 1). A viewPatch is a
list of 2D coordinates in counterclockwise order forming the boundary
of a filled polygon representing a piece of visual geometry. '''
self.viewPatches = {}
self.viewPatchColors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick
# random colors for some bodies. Each body will be given
# a unique color when using this random generator, with
# each visual element of the body colored the same.
color = iter(plt.cm.rainbow(np.linspace(0, 1,
load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch = np.vstack((
geom.float_data[0]/2.*np.array([1, 1, 1, 1,
-1, -1, -1, -1]),
geom.float_data[1]/2.*np.array([1, 1, 1, 1,
-1, -1, -1, -1]),
geom.float_data[2]/2.*np.array([1, 1, -1, -1,
-1, -1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
sample_pts = np.arange(0., 2.*math.pi, 0.25)
patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3]
+ math.sin(pt)*self.Tview[1, 0:3]
for pt in sample_pts])
patch = np.transpose(patch)
patch *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# I don't have access to the body to world transform
# yet; decide between drawing a box and circle assuming the
# T_body_to_world is will not rotate us out of the
# viewing plane.
z_axis = np.matmul(self.Tview[0:2, 0:3],
element_local_tf.multiply([0, 0, 1]))
if np.linalg.norm(z_axis) < 0.01:
# Draw a circle.
sample_pts = np.arange(0., 2.*math.pi, 0.25)
patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3]
+ math.sin(pt)*self.Tview[1, 0:3]
for pt in sample_pts])
patch = np.transpose(patch)
patch *= radius
else:
# Draw a bounding box.
patch = np.vstack((
radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]),
radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]),
(length/2)*np.array([1, 1, -1, -1, -1, -1, 1, 1])))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
patch = np.vstack((patch, np.ones((1, patch.shape[1]))))
patch = np.dot(element_local_tf.GetAsMatrix4(), patch)
# Project into 2D
patch = np.dot(self.Tview, patch)
# Close path if not closed
if (patch[:, -1] != patch[:, 0]).any():
patch = np.hstack((patch, patch[:, 0][np.newaxis].T))
this_body_patches.append(patch)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self.viewPatches[link.name] = this_body_patches
self.viewPatchColors[link.name] = this_body_colors
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()
