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