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()