def Rot_k_phi(self, k_vec, phi): #Eigen::Vector3d k_vec,float phi #Eigen::Matrix3d R_k_phi; kx = k_vec[0] ky = k_vec[1] kz = k_vec[2] #K_nrow = Vector(0.0, -kz, ky) #K_trow = Vector(kz, 0.0, -kx) #K_brow = Vector(-ky, kx, 0.0) N = 3 K = Rotation(0.0,-kz,ky, kz,0.0,-kx, -ky,kx,0.0) print "K",K Ksquare = K*K print "K^2",Ksquare I = Rotation() print "I",I K_Frame = Frame(K) print "K_Frame",K_Frame K_Frame_numpy = posemath.toMatrix(K_Frame) print "K_Frame_numpy",K_Frame_numpy K_numpy = K_Frame_numpy[0:3,0:3] print "K_numpy",K_numpy I_numpy = np.identity(N) print "I_numpy",I_numpy K_square_Frame = Frame(Ksquare) print "K_square_Frame",K_square_Frame K_square_Frame_numpy = posemath.toMatrix(K_square_Frame) print "K_square_Frame",K_square_Frame K_square_numpy = K_square_Frame_numpy[0:3,0:3] I_numpy = np.identity(N) print "math.sin(phi)",(math.sin(phi)) print "1-math.cos(phi)",(1-math.cos(phi)) print "K_numpy*K_numpy",K_numpy*K_numpy R_k_phi_numpy = I_numpy + math.sin(phi)*K_numpy + (1-math.cos(phi))*K_numpy*K_numpy print "R_k_phi_numpy",R_k_phi_numpy R_k_phi_numpy_FrameTemp = np.c_[R_k_phi_numpy, np.ones(N)] print "R_k_phi_numpy_FrameTemp",R_k_phi_numpy_FrameTemp R_k_phi_numpy_Frame = np.r_[R_k_phi_numpy_FrameTemp,[R_k_phi_numpy_FrameTemp[1]]] print "R_k_phi_numpy_Frame",R_k_phi_numpy_Frame R_k_phi_Frame = posemath.fromMatrix(R_k_phi_numpy_Frame) print "R_k_phi_Frame",R_k_phi_Frame R_k_phi = R_k_phi_Frame.M print "R_k_phi",R_k_phi R_k_phi_numpy_square = I_numpy + math.sin(phi)*K_numpy + (1-math.cos(phi))*K_square_numpy print "R_k_phi_numpy_square",R_k_phi_numpy_square R_k_phi_numpy_FrameTemp_square = np.c_[R_k_phi_numpy_square, np.ones(N)] print "R_k_phi_numpy_FrameTemp_square",R_k_phi_numpy_FrameTemp_square R_k_phi_numpy_Frame_square = np.r_[R_k_phi_numpy_FrameTemp_square,[R_k_phi_numpy_FrameTemp_square[1]]] print "R_k_phi_numpy_Frame_square",R_k_phi_numpy_Frame_square R_k_phi_Frame_square = posemath.fromMatrix(R_k_phi_numpy_Frame_square) print "R_k_phi_Frame",R_k_phi_Frame_square R_k_phi_square = R_k_phi_Frame_square.M print "R_k_phi",R_k_phi_square return R_k_phi
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. plan a path to the a place 15cm back from the new pose """ print 'regular move:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) try: hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase)) bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal", "armbase") self.global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = self.global_data.listener.lookupTransform( 'armbase', 'arm_goal', now) armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_world_goal failed: error %s.' % e)
def imageProc(self, image): if self.masterWidget is not None: return image # Get current force fmsg = self.camSync.getMsg(self.forceTopic) if type(fmsg) is not WrenchStamped: return image force = [fmsg.wrench.force.x, fmsg.wrench.force.y, fmsg.wrench.force.z] force = np.linalg.norm(force) targetF = 3 # Newtons targetMax = 6 # Newtons # Calculate color xp = [0, targetF, targetMax] fp = [0, 1, 0] colorPos = np.interp(force, xp, fp) color = colorsys.hsv_to_rgb(colorPos**3 / 3, .8, 1) # Get robot pose posMsg = self.camSync.getMsg(self.dvrkTopic) if type(posMsg) is not PoseStamped: return image pos = posemath.fromMsg(posMsg.pose) if self.drawType == "arrow": self.arrowActor.GetProperty().SetColor(color[0], color[1], color[2]) # Calculate pose of arrows initialRot = PyKDL.Frame(PyKDL.Rotation.RotY(np.pi / 2), PyKDL.Vector(0, 0, 0)) pos = pos * initialRot pos = self.cameraTransform.Inverse() * pos posMat = posemath.toMatrix(pos) posMatTarget = posMat.copy() # Scale arrows posMat[0:3, 0:3] = posMat[0:3, 0:3] * .025 * targetF setActorMatrix(self.targetActor, posMat) posMat[0:3, 0:3] = posMat[0:3, 0:3] * force / targetF setActorMatrix(self.arrowActor, posMat) elif self.drawType == "bar": self.forceBar.GetProperty().SetColor(color[0], color[1], color[2]) # Move background bar pos = self.cameraTransform.Inverse() * pos pos2 = PyKDL.Frame(PyKDL.Rotation.Identity(), pos.p) pos2.M.DoRotZ(np.pi) pos2.p = pos2.p + pos2.M.UnitX() * -.015 # + pos2.M.UnitZ() * 0.01 posMat = posemath.toMatrix(pos2) setActorMatrix(self.bar, posMat) setActorMatrix(self.greenLine, posMat) # Scale color bar fp2 = [0, .5, 1] scalePos = np.interp(force, xp, fp2) posMat[1, 0:3] = posMat[1, 0:3] * scalePos setActorMatrix(self.forceBar, posMat) return image
def draw_raw_map(): global package_path global arucos_dict global BASE_ARUCO_ID global rviz_marker_pub global aruco_map while(rviz_marker_pub.get_num_connections() < 1): if rospy.is_shutdown(): rospy.signal_shutdown('Master主节点没有开启') # load_aruco_map() rospy.logwarn("请创建RVIZ的Subscriber") rospy.sleep(5) aruco_map[BASE_ARUCO_ID] = get_base_tag_pose() # 绘制参考marker draw_single_marker(BASE_ARUCO_ID, get_base_tag_pose(), alpha=0.8, height=0.05) # 构建一个图 graph = aruco_udg(max_dis=rospy.get_param('max_distance')) order = graph.BFS() for aruco_id in order[1:]: # 跳过第一个, 因为第一个是base aruco, 已经确定 # 按照拓扑顺序依次遍历 parent_id = graph.nodes_dict[aruco_id].parent_id # 获取所有记录到的从parent变换到当前码的变换 pose_list = arucos_dict[parent_id][aruco_id] # 均值滤波获取Pose pose = weighted_mean_filter(pose_list) # 父亲码在世界坐标系下的位姿 parent_pose = aruco_map[parent_id] world2parent_mat = pm.toMatrix(pm.fromMsg(parent_pose)) parent2child_mat = pm.toMatrix(pm.fromMsg(pose)) world2child_mat = world2parent_mat.dot(parent2child_mat) aruco_pose = pm.toMsg(pm.fromMatrix(world2child_mat)) # 地图追加该aruco码 aruco_map[aruco_id] = aruco_pose # 绘制当前的aruco码 draw_single_marker(aruco_id, aruco_pose, alpha=0.8, height=0.05) # 序列化保存 with open(package_path + '/data/aruco_map.bin', 'wb') as f: pickle.dump(aruco_map, f) # 发布静态TF变换 for aruco_id, pose in aruco_map.items(): send_aruco_static_transform(aruco_id, pose) # 结束当前的进程 rospy.signal_shutdown('绘制了所有的aruco,并发布了aruco码的TF变换')
def optitrack_cb(self, msg): if msg.header.frame_id != self.global_frame: return for rigid_body in msg.bodies: if rigid_body.tracking_valid: if rigid_body.id == self.table_id: frame = posemath.fromMsg(rigid_body.pose) self.Ttable = posemath.toMatrix(frame) if rigid_body.id == self.stick_id: frame = posemath.fromMsg(rigid_body.pose) self.Tshort = posemath.toMatrix(frame)
def align_pose(self, pose): objectInCamera = pm.toMatrix(pm.fromMsg(pose)) ModelRecManager.tf_listener.waitForTransform("/world", "/camera_rgb_optical_frame", rospy.Time(0), rospy.Duration(5)) cameraInWorld = pm.toMatrix(pm.fromTf(ModelRecManager.tf_listener.lookupTransform("/world", "/camera_rgb_optical_frame",rospy.Time(0)))) objectInWorld = dot(cameraInWorld, objectInCamera) """45 degrees rotated around z axis""" objectInWorld[0:3,0:3] = mat([[0.7071,-0.7071,0], [0.7071,0.7071,0], [0,0,1]]); worldInCamera = linalg.inv(cameraInWorld) objectInCameraModified = dot(worldInCamera, objectInWorld) return pm.toMsg(pm.fromMatrix(objectInCameraModified))
def omni_callback(joint_state): global update_pub, last_button_state sendTf(marker_tf, '/marker', fixed_frame) sendTf(zero_tf, '/base', fixed_frame) sendTf(marker_ref, '/marker_ref', fixed_frame) sendTf(stylus_ref, '/stylus_ref', fixed_frame) try: update = InteractionCursorUpdate() update.pose.header = std_msgs.msg.Header() update.pose.header.stamp = rospy.Time.now() update.pose.header.frame_id = 'marker_ref' if button_clicked and last_button_state == update.GRAB: update.button_state = update.KEEP_ALIVE elif button_clicked and last_button_state == update.KEEP_ALIVE: update.button_state = update.KEEP_ALIVE elif button_clicked: update.button_state = update.GRAB elif last_button_state == update.KEEP_ALIVE: update.button_state = update.RELEASE else: update.button_state = update.NONE updateRefs() update.key_event = 0 control_tf = ((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, control_offset)) if button_clicked: # Get pose corresponding to transform between stylus reference and current position. stylus_tf = listener.lookupTransform('/stylus_ref', '/stylus', rospy.Time(0)) stylus_matrix = pm.toMatrix(pm.fromTf(stylus_tf)) control_matrix = pm.toMatrix(pm.fromTf(control_tf)) p = pm.toMsg(pm.fromMatrix(numpy.dot(control_matrix, stylus_matrix))) else: p = pm.toMsg(pm.fromTf(control_tf)) # Simply scale this up a bit to increase the workspace. workspace = 4 p.position.x = p.position.x * workspace p.position.y = p.position.y * workspace p.position.z = p.position.z * workspace update.pose.pose = p last_button_state = update.button_state # Publish feedback. update_pub.publish(update) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr("Couldn't look up transform. These things happen...")
def calculate_psmb_world_transform(self): self.trans_psmb_world_l = self.trans_ecmb_psmb_l.Inverse( ) * self.trans_ecmb_ecm * self.trans_camleft_ecm.Inverse( ) * self.trans_camleft_world self.trans_psmb_world_r = self.trans_ecmb_psmb_l.Inverse( ) * self.trans_ecmb_ecm * self.trans_camright_ecm.Inverse( ) * self.trans_camright_world z = { "T_from_left": pm.toMatrix(self.trans_psmb_world_l).tolist(), "T_from_right": pm.toMatrix(self.trans_psmb_world_r).tolist() } with open("./calib_parameters/z_estimation.json", "w") as f: z = json.dumps(z, indent=4) f.write(z)
def transfrom_between_arucos(camera2ai, camera2aj): ''' 计算arucos之间的转换 换算成以 aruco i为参考坐标系 对应arucoj的姿态 ''' # 获取以相机为世界坐标系下的码j的位姿 camera2aj_mat = pm.toMatrix(camera2aj) # 获取以码i做世界坐标系下的相机的位姿 ai2camera_mat = pm.toMatrix(camera2ai.Inverse()) # 计算aj在ai下的位姿 位姿传递 ai2aj_mat = ai2camera_mat.dot(camera2aj_mat) # 生成码j在码i坐标系下的位姿 对应的KDL Frame对象 ai2aj_frame = pm.fromMatrix(ai2aj_mat) # 返回对象 return pm.toMsg(ai2aj_frame)
def processMarkerFeedback(feedback): global marker_tf, marker_name marker_name = feedback.marker_name marker_offset_tf = ((0, 0, marker_offset), tf.transformations.quaternion_from_euler(0, 0, 0)) marker_tf = pm.toTf(pm.fromMatrix(numpy.dot(pm.toMatrix(pm.fromMsg(feedback.pose)), pm.toMatrix(pm.fromTf(marker_offset_tf))))) if feedback.menu_entry_id: marker_tf = zero_tf
def ground_area(cm, src, tf_x): """ Compute 4-corners of ground-plane from camera. Args: cm(image_geometry.PinholeCameraModel): camera model for image. src(np.ndarray): [4,2] array of four corners, formatted (u,v). Represents the area in the frame to compute the groundplane projection. tf_x(tuple): (txn,qxn) tuple for translation and rotation(quaternion). Refer to results from tf.TransformListener.lookupTransform() Returns: points(np.ndarray): [4,2] array of four corners, formatted (x,y) z value is implicitly zero w.r.t. the source tf frame. """ txn, qxn = tf_x # K = 3x3 camera projection w, h = cm.width, cm.height M_x = pm.toMatrix(pm.fromTf(tf_x)) cray = [cm.projectPixelTo3dRay(e) for e in src] # [4,3] formatted (x,y,z) ?? # convert ray to map coord mray = np.dot(cray, M_x[:3, :3].T) # right-multiply rotation matrix # extend ray to groundplane l = -txn[2] / mray[:, 2] # need to flip z since mray is pointing downwards # i.e. same as mray[:,2].dot([0,0,-1]) which is the correct distance gray = l[:, np.newaxis] * mray return gray[:, :2] + np.reshape(txn[:2], [-1, 2])
def step(self): if self.T_ is None: self.init_tf() return if self.recv_: self.recv_ = False pose = self.odom_.pose.pose T0 = pm.toMatrix(pm.fromMsg(pose)) # android_odom --> android # send odom -> base_link transform T = tf.transformations.concatenate_matrices(self.T_, T0, self.Ti_) frame = pm.fromMatrix(T) if self.pub_tf_: txn, qxn = pm.toTf(frame) self.tfb_.sendTransform(txn, qxn, self.odom_.header.stamp, 'odom', 'base_link') # send as msg # TODO : does not deal with twist/covariance information msg = pm.toMsg(frame) self.cvt_msg_.pose.pose = pm.toMsg(frame) self.cvt_msg_.header.frame_id = 'map' # experimental self.cvt_msg_.header.stamp = self.odom_.header.stamp self.pub_.publish(self.cvt_msg_)
def simple_horiz_kvec_motion_psm2(self, O_needle, r_needle, kvec_yaw, gripper_affines_wrt_camera): dphi = math.pi/40.0 bvec0 = Vector(1,0,0) nvec = Vector(0,0,1) bvec = self.Rotz(kvec_yaw)*bvec0 tvec = bvec*nvec R0 = Rotation(nvec,tvec,bvec) R0 = R0.Inverse() tip_pos = O_needle - r_needle*tvec self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos self.affine_gripper_frame_wrt_camera_frame_.M = R0 del gripper_affines_wrt_camera[:] nsolns = 0 nphi = 0 print "nphi: " for phi in range(0.0,-math.pi,-dphi): R = self.Rot_k_phi(bvec, phi)*R0 self.affine_gripper_frame_wrt_camera_frame_.M=R R_Frame = posemath.toMatrix(self.affine_gripper_frame_wrt_camera_frame_) R_column2_numpy = R_Frame[0:3,1] R_column2 = Vector(R_column2_numpy[0],R_column2_numpy[1],R_column2_numpy[2]) tip_pos = O_needle - r_needle*R_column2 self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos des_gripper1_wrt_base = self.default_affine_lcamera_to_psm_two_.Inverse()*self.affine_gripper_frame_wrt_camera_frame_ ''' ERDEM if (ik_solver_.ik_solve(des_gripper1_wrt_base)) { nsolns++; cout<<nphi<<","; #cout<<": found IK; nsolns = "<<nsolns<<endl; gripper_affines_wrt_camera.push_back(affine_gripper_frame_wrt_camera_frame_); }''' nphi += 1 print "\n"
def setArmPosition(self, pos, preferedAngs=None): self.currentPos = np.array(pos) if not self.useOptim: angs = self.__oldIVK__(pos, preferedAngs) else: print(pm.toMatrix(pm.fromMsg(self.ur2ros(pos)))) trj = trajectoryOptim(self.lastJointAngles, pm.toMatrix(pm.fromMsg(self.ur2ros(pos)))) trj._numSteps = 1 angs = trj.optimize() loc = angs[-1] angs = angs[0][0] self.lastJointAngles = angs self.__publish__(angs) self.currentPos[0:3] = loc[0][0:-1,3] return loc[0][0:-1,3]
def _makePose(self, pose_msg): ''' Read in a pose message and convert it to a standard data format. We use KDL Frames for fast operations down the line. If we previously set the `from_unity` flag to true, then we need to adjust our frames so that they make sense as well. Parameters: ----------- pose_msg: a ROS pose message Returns: -------- a KDL pose in right hand, z-up notation ''' pose = pm.fromMsg(pose_msg) if self.from_unity: H = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) x = pm.toMatrix(pose) x = H.dot(x) pose = pm.fromMatrix(x) return pose
def get_staubli_cartesian_as_tran(): """@brief - Read the staubli end effector's cartesian position as a 4x4 numpy matrix Returns 4x4 numpy message on success, empty message on failure """ current_pose = get_staubli_cartesian_as_pose_msg() return pm.toMatrix(pm.fromMsg(current_pose))
def align_pose(self, pose): objectInCamera = pm.toMatrix(pm.fromMsg(pose)) ModelRecManager.tf_listener.waitForTransform( "/world", "/camera_rgb_optical_frame", rospy.Time(0), rospy.Duration(5)) cameraInWorld = pm.toMatrix( pm.fromTf( ModelRecManager.tf_listener.lookupTransform( "/world", "/camera_rgb_optical_frame", rospy.Time(0)))) objectInWorld = dot(cameraInWorld, objectInCamera) """45 degrees rotated around z axis""" objectInWorld[0:3, 0:3] = mat([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0, 1]]) worldInCamera = linalg.inv(cameraInWorld) objectInCameraModified = dot(worldInCamera, objectInWorld) return pm.toMsg(pm.fromMatrix(objectInCameraModified))
def imageProc(self, image): # Get current force force = self.currentForce force = np.linalg.norm(force) targetF = 4 # Newtons targetR = 1 # Newtons # Calculate color xp = [targetF - targetR, targetF, targetF + targetR] fp = [0, 1, 0] colorPos = np.interp(force, xp, fp) color = colorsys.hsv_to_rgb(colorPos**3 / 3, .8, 1) if self.drawType == "arrow": self.arrowActor.GetProperty().SetColor(color[0], color[1], color[2]) # Calculate pose of arrows initialRot = PyKDL.Frame(PyKDL.Rotation.RotY(np.pi / 2), PyKDL.Vector(0, 0, 0)) pos = self.robot.get_current_position() * initialRot pos = self.cameraTransform.Inverse() * pos posMat = posemath.toMatrix(pos) posMatTarget = posMat.copy() # Scale arrows posMat[0:3, 0:3] = posMat[0:3, 0:3] * .025 * targetF setActorMatrix(self.targetActor, posMat) posMat[0:3, 0:3] = posMat[0:3, 0:3] * force / targetF setActorMatrix(self.arrowActor, posMat) elif self.drawType == "bar": self.forceBar.GetProperty().SetColor(color[0], color[1], color[2]) # Move background bar pos = self.robot.get_current_position() pos = self.cameraTransform.Inverse() * pos pos2 = PyKDL.Frame(PyKDL.Rotation.Identity(), pos.p) pos2.M.DoRotZ(np.pi) pos2.p = pos2.p + pos2.M.UnitX() * -.015 posMat = posemath.toMatrix(pos2) setActorMatrix(self.bar, posMat) setActorMatrix(self.greenLine, posMat) # Scale color bar fp2 = [0, .5, 1] scalePos = np.interp(force, xp, fp2) print scalePos posMat[1, 0:3] = posMat[1, 0:3] * scalePos setActorMatrix(self.forceBar, posMat) return image
def getCartesianMove(self, frame, q0, base_steps=1000, steps_per_meter=1000, time_multiplier=10): # interpolate between start and goal pose = pm.fromMatrix(self.kdl_kin.forward(q0)) cur_rpy = np.array(pose.M.GetRPY()) cur_xyz = np.array(pose.p) goal_rpy = np.array(frame.M.GetRPY()) goal_xyz = np.array(frame.p) steps = base_steps + int((pose.p - frame.p).Norm() * steps_per_meter) print " -- Computing %f steps" % steps ts = (pose.p - frame.p).Norm() / steps * time_multiplier traj = JointTrajectory() traj.points.append( JointTrajectoryPoint(positions=q0, velocities=[0] * len(q0), accelerations=[0] * len(q0))) # compute IK for i in range(1, steps + 1): xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz)) rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy)) frame = pm.toMatrix( kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]), kdl.Vector(xyz[0], xyz[1], xyz[2]))) #q = self.kdl_kin.inverse(frame,q0) q = self.ik(frame, q0) if self.verbose: print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q)) if not q is None: pt = JointTrajectoryPoint(positions=q, velocities=[0] * len(q), accelerations=[0] * len(q)) pt.time_from_start = rospy.Duration(i * ts) traj.points.append(pt) q0 = q elif i == steps: return JointTrajectory() if len(traj.points) < base_steps: print rospy.logerr("Planning failure with " \ + str(len(traj.points)) \ + " / " + str(base_steps) \ + " points.") return JointTrajectory() traj.joint_names = self.joint_names return traj
def calculate_ecmb_world_transform(self): #ECM matrix self.trans_ecmb_world_l = self.trans_ecmb_ecm * self.trans_camleft_ecm.Inverse( ) * self.trans_camleft_world self.trans_ecmb_world_r = self.trans_ecmb_ecm * self.trans_camright_ecm.Inverse( ) * self.trans_camright_world # print(self.trans_ecmb_world_l) # print(self.trans_ecmb_world_r) Y = { "T_from_left": pm.toMatrix(self.trans_ecmb_world_l).tolist(), "T_from_right": pm.toMatrix(self.trans_ecmb_world_r).tolist() } with open("./calib_parameters/y_estimation.json", "w") as f: Y = json.dumps(Y, indent=4) f.write(Y)
def get_err_twist(self, goal_frame=None, dT=0.01): """ Get the desirable twist for given error. """ #_cur_frame = self.get_current_frame() if self.btn_state_2 == 1: b_T_saw0 = pm.toMatrix(self.cur_frame) b_T_vr0 = pm.toMatrix(self._frame2) self.vr0_T_saw0 = np.matmul(inv(b_T_vr0), b_T_saw0) # numpy 4x4 array # else: # self.vr0_T_saw0 = np.identity(4) self._frame2 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2))) return PyKDL.diff(self.cur_frame, self._frame2, dT)
def compute_needle_drive_gripper_affines(self, gripper_affines_wrt_camera, gripper_affines_wrt_psm): phi_insertion_ = 0.0 self.affine_needle_frame_wrt_tissue_ = self.affine_init_needle_frame_wrt_tissue_ dphi = math.pi/(2.0*(NSAMPS_DRIVE_PLAN-1)) self.R0_needle_wrt_tissue_ = self.affine_needle_frame_wrt_tissue_.M self.affine_needle_frame_wrt_tissue_.M = self.Rotx(self.psi_needle_axis_tilt_wrt_tissue_)*self.R0_needle_wrt_tissue_ self.R0_needle_wrt_tissue_= self.affine_needle_frame_wrt_tissue_.M kvec_needle_Frame = posemath.toMatrix(self.affine_needle_frame_wrt_tissue_) kvec_needle_numpy = kvec_needle_Frame[0:3,2] kvec_needle = Vector(kvec_needle_numpy[0],kvec_needle_numpy[1],kvec_needle_numpy[2]) if(debug_needle_print): print "kvec_needle=" , kvec_needle if(debug_needle_print): print "R0 needle:" needle_origin = self.affine_needle_frame_wrt_tissue_.p self.affine_needle_frame_wrt_tissue_.p = self.Rotx(self.psi_needle_axis_tilt_wrt_tissue_)*needle_origin; if(debug_needle_print): print self.affine_needle_frame_wrt_tissue_.M for ipose in range(0,NSAMPS_DRIVE_PLAN): Rot_needle = self.Rot_k_phi(kvec_needle,phi_insertion_) #R_needle_wrt_tissue_ = Roty_needle*R0_needle_wrt_tissue_; #update rotation of needle drive self.R_needle_wrt_tissue_ = Rot_needle*self.R0_needle_wrt_tissue_ #update rotation of needle drive if(debug_needle_print): print "R_needle w/rt tissue:" if(debug_needle_print): print self.R_needle_wrt_tissue_ #need to check these transforms... self.affine_needle_frame_wrt_tissue_.M = self.R_needle_wrt_tissue_ self.affine_gripper_frame_wrt_tissue_ = self.affine_needle_frame_wrt_tissue_*self.affine_needle_frame_wrt_gripper_frame_.Inverse() if(debug_needle_print): print("affine_gripper_frame_wrt_tissue_") if(debug_needle_print): print_affine(self.affine_gripper_frame_wrt_tissue_) self.affine_gripper_frame_wrt_camera_frame_ = self.affine_tissue_frame_wrt_camera_frame_*self.affine_gripper_frame_wrt_tissue_ if(debug_needle_print): print("affine_gripper_frame_wrt_camera_frame_") print_affine(self.affine_gripper_frame_wrt_camera_frame_) gripper_affines_wrt_camera.append(self.affine_gripper_frame_wrt_camera_frame_) self.affine_gripper_frame_wrt_psm_frame_ = self.default_affine_lcamera_to_psm_one_.Inverse()*self.affine_gripper_frame_wrt_camera_frame_ gripper_affines_wrt_psm.append(self.affine_gripper_frame_wrt_psm_frame_) phi_insertion_+=dphi
def calc_joint_vel_3(self): """ calc joint vel using PyKDL IK """ with self.mutex: q_now = self.joints_to_kdl(type='positions', values=None) T_sd = self._get_goal_matrix() # targ_fr = self.get_frame(T_sd) dT = self.get_dt() err_mat = np.dot(np.linalg.inv(pm.toMatrix(self.get_current_frame())), pm.toMatrix(targ_fr)) # err_twist = self.to_kdl_twist(r.se3ToVec(r.MatrixLog6(err_mat)).tolist()) # cbModern robotics pp 230 22Nff # err_twist = self.get_err_twist(targ_fr, dT) # err term twist(Xd - X) err_twist = self.get_err_twist(targ_fr, 1.0) # err term twist(Xd - X) # err_twist = pm.fromMatrix(cur_fk) * _err_twist # err_twist *= 0.0 # des_twist = self.get_des_twist(dT) # feed-forward twist value Vd_dot des_twist = self.get_des_twist(dT) # feed-forward twist value Vd_dot # des_twist = pm.fromMatrix(cur_fk) * _des_twist # des_twist = self.adj_to_twist(err_mat, _des_twist) # des_twist =self.to_kdl_twist(des_twist) # rospy.logwarn('=============== Twist error ===============') # print (err_twist) # rospy.logwarn('=============== Twist desired ===============') # print (des_twist, self.cur_wp_idx) integ_twist = self.integ_error(err_twist, dT) # rospy.logwarn('=============== Twist integrated ===============') # print (integ_twist) err_twist, integ_twist = self.apply_gain(err_twist, integ_twist) # total_twist = des_twist + err_twist + integ_twist # FF + Pg*Err + Ig*Integ(Err) total_twist = err_twist + integ_twist # FF + Pg*Err + Ig*Integ(Err) # self.q_dot = self.kdl_inv_vel_kine(cur_joint_pos=q_now, ee_twist=total_twist) self.q_dot = self.kdl_inv_vel_kine(cur_joint_pos=q_now, ee_twist=total_twist) self.q_dot = self.scale_joint_vel(self.q_dot) # publish joint command qdot_output = dict(zip(self.names, self.q_dot)) # rospy.logwarn('=============== Computed joint vels ===============') # print (qdot_output) self.prev_frame = self.cur_frame # the call of this line seems crucial!! self.limb.set_joint_velocities(qdot_output) # rospy.logwarn('=============== Twist actual ===============') # print (self.ee_ang_twist + self.ee_lin_twist) self._check_traj(dT) self.last_time = self.current_time
def __getstate__(self): state = {} state['model_name'] = self.model_name state['object_name'] = self.object_name buff = StringIO.StringIO() self.point_cloud_data.serialize(buff) buff.seek(0) state['point_cloud_data'] = buff.readlines() state['pose'] = pm.toMatrix(pm.fromMsg(self.pose)) return state
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. release the hand 2. backup the hand 3. plan a path to the a place 15cm back from the new pose 4. use guarded motion to move forward """ print 'adjustment:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -.05], [0, 0, 0, 1]]) back_from_arm_goal = arm_goal * backup #move back by 10cm #move_forward(-.1) #raw_input("Press enter to go to the target pose...") #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) send_cartesian_goal(back_from_arm_goal, True) raw_input("Press enter to use guarded motion to move forward...") #guarded move forward tp.MoveHandSrv(1, [0, 0, 0, grasp_msg.pre_grasp_dof[0]]) GuardedMoveUntilHit(self.global_data, array([0, 0, 1]), 'PALM', 0.05, 20) raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #tp.move_hand_velocity([0.5,0.5,0.5,0.0]) GuardedCloseHand(self.global_data) selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' success = tp.lift_arm(.1, True) if not success: grasp_status = graspit_msgs.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object'
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. release the hand 2. backup the hand 3. plan a path to the a place 15cm back from the new pose 4. use guarded motion to move forward """ print 'adjustment:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1,0,0,0], [0,1,0,0], [0,0,1,-.05], [0,0,0,1]]); back_from_arm_goal = arm_goal * backup #move back by 10cm #move_forward(-.1) #raw_input("Press enter to go to the target pose...") #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) send_cartesian_goal(back_from_arm_goal, True) raw_input("Press enter to use guarded motion to move forward...") #guarded move forward tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]]) GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #tp.move_hand_velocity([0.5,0.5,0.5,0.0]) GuardedCloseHand(self.global_data) selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' success = tp.lift_arm(.1, True) if not success: grasp_status = graspit_msgs.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object'
def getposecb(self, a, b): print "Setting Pose based on world model" if((rospy.Time.now().to_sec() - b.pose.header.stamp.to_sec()) > 4.0): print "time now = ", rospy.Time.now().to_sec(), "time from pose = ", b.pose.header.stamp.to_sec() print "Time stamp of detection to old: diff=", (rospy.Time.now().to_sec() - b.pose.header.stamp.to_sec()) if(self.detection_counter <= 4): self.detection_counter += 1 return "preempted" else: self.detection_counter = 0 return "aborted" self.config_over_object.header.stamp = rospy.Time.now() self.config_over_object.pose.position.x = b.pose.pose.position.x self.config_over_object.pose.position.y = b.pose.pose.position.y #ao = PyKDL.Rotation.Quaternion(self.config_over_object.pose.orientation.x, self.config_over_object.pose.orientation.y, self.config_over_object.pose.orientation.z, self.config_over_object.pose.orientation.w) #ao = PyKDL.Rotation.Quaternion(self.config_over_object.pose.orientation.x, self.config_over_object.pose.orientation.y, self.config_over_object.pose.orientation.z, self.config_over_object.pose.orientation.w) #bo = PyKDL.Rotation.Quaternion(b.pose.pose.orientation.x, b.pose.pose.orientation.y, b.pose.pose.orientation.z, b.pose.pose.orientation.w) #c = ao #* bo#.Inverse() m_manip = pm.toMatrix( pm.fromMsg(self.config_over_object.pose) ) m_obj = pm.toMatrix( pm.fromMsg(b.pose.pose) ) import numpy from tf.transformations import * m_ori = pm.toMatrix( pm.fromTf( ((0,0,0), quaternion_from_euler(0, 0, math.radians(-20.0))) )) box_pose = pm.toMsg( pm.fromMatrix(numpy.dot(m_manip,m_obj)) ) m_test = numpy.dot(m_manip,m_obj) newrot = pm.toMsg( pm.fromMatrix(m_test)) print newrot print self.config_over_object.pose #print c.GetQuaternion() self.config_over_object.pose.orientation = box_pose.orientation #self.config_over_object.pose.orientation.y = c.GetQuaternion()[1] #self.config_over_object.pose.orientation.z = c.GetQuaternion()[2] #self.config_over_object.pose.orientation.w = c.GetQuaternion()[3] #self.config_over_object.pose.position.z = 0.30#0.430 self.config_object.header.stamp = rospy.Time.now() self.config_object.pose.position.x = b.pose.pose.position.x self.config_object.pose.position.y = b.pose.pose.position.y #self.config_object.pose.position.z = 0.2 #0.095 self.config_object.pose.orientation = box_pose.orientation
def pose_callback(self, pose): if self.T_cam2base is None: try: tfpose = self.tf_listener.lookupTransform('/camera', '/base_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("failed to find tf between base_link and camera") return self.T_cam2base = pm.toMatrix(pm.fromTf(tfpose)) self.t_cam2base = self.T_cam2base[0:3, 3] self.R_cam2base = self.T_cam2base[0:3, 0:3].dot(ez.eulerZYX(0.05, -0.13, 0)) self.compute_pose_label(self.label)
def test_compute_twist(self): for _ in range(100): T0 = br.transform.random() vel = np.random.rand(3) rot = np.random.rand(3) kdl_twist = PyKDL.Twist(PyKDL.Vector(*vel), PyKDL.Vector(*rot)) F0 = posemath.fromMatrix(T0) F1 = PyKDL.addDelta(F0, kdl_twist) T1 = posemath.toMatrix(F1) twist = ru.transforms.compute_twist(F0, F1) np.testing.assert_allclose(twist, np.hstack((vel, rot)))
def update(self, dt=0.0): # initial pose T0 = pm.toMatrix(pm.fromMsg(self._odom.pose.pose)) dx = self._cmd_vel.linear dq = self._cmd_vel.angular dT = tx.compose_matrix(angles=(dq.x * dt, dq.y * dt, dq.z * dt), translate=(dx.x * dt, dx.y * dt, dx.z * dt)) T1 = tx.concatenate_matrices(T0, dT) self._odom.pose.pose = pm.toMsg(pm.fromMatrix(T1))
def test_roundtrip(self): c = Frame() d = pm.fromMsg(pm.toMsg(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromMatrix(pm.toMatrix(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromTf(pm.toTf(c)) self.assertEqual(repr(c), repr(d))
def Rel2Globalfile(relfname, globalfname): pose_array = PoseArray() Rel2Global(relfname, pose_array) with open(globalfname, "w") as relfile: for pose in pose_array.poses: outarray = posemath.toMatrix(posemath.fromMsg(pose)) outonedim = outarray[0:3, :].reshape((12)) for index in range(11): relfile.write("{} ".format(outonedim[index])) relfile.write("{}\n".format(outonedim[11])) relfile.close()
def ik(self, pose, q0): ''' The ik() command is used by various agents and problem domains to recover a command vector that will move the robot arm to the right pose. Params: -------- pose: kdl frame to move to q0: current joint position ''' return self.kinematics.inverse(pm.toMatrix(pose), q0)
def get_des_twist(self, dT=0.01): """ Get desired twist @ time t, by computing differential of traj_frame @ t=T-1 and t=T """ #_cur_frame = self.get_current_frame() #return PyKDL.diff(self.prev_frame, _cur_frame, dT) # # if self.btn_state_2 == 1: b_T_saw0 = pm.toMatrix(self.cur_frame) b_T_vr0 = pm.toMatrix(self._frame2) self.vr0_T_saw0 = np.matmul(inv(b_T_vr0), b_T_saw0) # numpy 4x4 array # else: # self.vr0_T_saw0 = np.identity(4) self._frame2 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame2))) self._frame1 = pm.fromMatrix( np.matmul(self.vr0_T_saw0, pm.toMatrix(self._frame1))) return PyKDL.diff(self._frame1, self._frame2, dT)
def pregrasp_object(global_data, object_name, adjust_height, grasp_tran, object_height_col=2, pregrasp_dist=-.05, object_height_adjustment=0.0, run_it=True): """@brief - Move the hand to the pregrasp position. This procedure is suprisingly intricate. Given the object filename and the grasp transform, this transforms the grasp to world coordinates then runs a planner to avoid collisions in moving the arm the long distance to the object. This uses the scene graph, which is currently provided by a camera. Returns whether the pregrasp location was reachable, the filename in which the trajectory from OpenRave is stored, and the list of the dof values that are contained in the trajectory. """ """ Sets up object location in openrave -- this code is all to compensate for the current object localization method only knowing the location of the base of the object - the height of the object's origin from the base must be used to adjust it's location in openrave before planning. """ print "Pregrasping object: %s" % (object_name) #Get the object transform in the world. obj_tran = pm.toMatrix( pm.fromTf( global_data.listener.lookupTransform("/world", object_name, rospy.Time(0)))) if (obj_tran == []): warn("Scene graph in TF is broken! Most\ likely, the object transform could not be found.") return [], [], [], [] """Set up and visualize the grasp transforms""" pregrasp_tran = get_pregrasp_tran_from_tran(grasp_tran, pregrasp_dist) publish_grasp_tran(pregrasp_tran) final_tran = dot(obj_tran, pregrasp_tran) publish_grasp_tran(final_tran) success = False """Run the planner""" plan = [] j = [] try: success, trajectory_filename, dof_list, j = run_cbirrt_with_tran( global_data.or_env, final_tran) except Exception, e: warn("Failed running planner with\ error %s" % e)
def analyze_demonstration_pose(self, demo_grasp): if(len(self.data) ==0): return 1.0 demo_pose = pm.toMatrix(pm.fromMsg(demo_grasp.final_grasp_pose)) demo_position = demo_pose[:3,3] distances, indices = self.model.kneighbors(demo_position) indices = unique(indices) nbrs = [t for t in itertools.compress(self.data, indices)] valid_nbrs = [] for n in nbrs: pose = pm.toMatrix(pm.fromMsg(n[0].final_grasp_pose)) if (acos(dot(pose[:3,2],demo_pose[:3,2]) < .52)): valid_nbrs.append(n) if len(valid_nbrs): success_probability = len([n for n in valid_nbrs if n[1] & 1])/(1.0*len(valid_nbrs)) else: success_probability = 0 self.demo_pose_analysis_publisher.publish(success_probability) return success_probability
def get_camera_pose(aruco_msg): ''' 根据aruco的Message,与aruco地图(aruco_map) 推算出aruco码相对于世界坐标系的位置跟姿态 ''' global aruco_map # 从姿态信息 构建 PyKDL Frame对象 aruco2camera = pm.fromMsg(aruco_msg.pose.pose) # 调整姿态 绕X轴逆向旋转90度 弧度 aruco2camera.M.DoRotX(-math.pi / 2) aruco2camera.M.DoRotZ(math.pi) # 校正姿态 Z轴旋转180度 # 矩阵取反,改为相机相对于aruco码的相对位置 camera2aruco = aruco2camera.Inverse() # 获取aruco相对于世界坐标系的矩阵 aruco2world_mat = pm.toMatrix(pm.fromMsg(aruco_map[aruco_msg.id])) # 矩阵相乘,获取相机相对于世界坐标系的变换矩阵 camera2world_mat = aruco2world_mat.dot(pm.toMatrix(camera2aruco)) # 将矩阵转换为姿态Message camera_pose_msg = pm.toMsg(pm.fromMatrix(camera2world_mat)) return camera_pose_msg
def analyze_demonstration_pose(self, demo_grasp): if (len(self.data) == 0): return 1.0 demo_pose = pm.toMatrix(pm.fromMsg(demo_grasp.final_grasp_pose)) demo_position = demo_pose[:3, 3] distances, indices = self.model.kneighbors(demo_position) indices = unique(indices) nbrs = [t for t in itertools.compress(self.data, indices)] valid_nbrs = [] for n in nbrs: pose = pm.toMatrix(pm.fromMsg(n[0].final_grasp_pose)) if (acos(dot(pose[:3, 2], demo_pose[:3, 2]) < .52)): valid_nbrs.append(n) if len(valid_nbrs): success_probability = len([n for n in valid_nbrs if n[1] & 1 ]) / (1.0 * len(valid_nbrs)) else: success_probability = 0 self.demo_pose_analysis_publisher.publish(success_probability) return success_probability
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. plan a path to the a place 15cm back from the new pose """ print 'regular move:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) try: hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase)) bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal", "armbase") self.global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = self.global_data.listener.lookupTransform('armbase', 'arm_goal', now) armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_world_goal failed: error %s.'%e)
def goto(kdl_kin, pub, listener, trans, rot): try: T = pm.fromTf((trans, rot)) q0 = [-1.0719114121799995, -1.1008140645600006, 1.7366724169200003, -0.8972388608399999, 1.25538042294, -0.028902652380000227,] # DEFAULT objt, objr = ((0.470635159016, 0.0047549889423, -0.428045094013),(0,0,0,1)) T_orig = pm.fromTf((objt,objr)) # MOVEd objt, objr = ((0.52, 0.00, -0.43),(0,0,0,1)) T_new = pm.fromTf((objt,objr)) T_pose = pm.toMatrix(T) Q = kdl_kin.inverse(T_pose, q0) print "----------------------------" print "[ORIG] Closest joints =", Q msg = JointState(name=CONFIG['joints'], position=Q) pub.publish(msg) rospy.sleep(0.2) T_goal = T_orig.Inverse() * T T2 = T_new * T_goal T2_pose = pm.toMatrix(T2) Q = kdl_kin.inverse(T2_pose, q0) print "[NEW] Closest joints =", Q msg = JointState(name=CONFIG['joints'], position=Q) pub.publish(msg) rospy.sleep(0.2) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e: pass
def model_state_publisher(self): ############# targetFrame = PyKDL.Frame(PyKDL.Rotation.Quaternion(self.target.pose.orientation.x,self.target.pose.orientation.y,self.target.pose.orientation.z,self.target.pose.orientation.w), PyKDL.Vector(self.target.pose.position.x,self.target.pose.position.y,self.target.pose.position.z)) targetFrameMatrix = pm.toMatrix(targetFrame) sourceFrame = PyKDL.Frame(PyKDL.Rotation.Quaternion(self.source.pose.orientation.x,self.source.pose.orientation.y,self.source.pose.orientation.z,self.source.pose.orientation.w),PyKDL.Vector(self.source.pose.position.x,self.source.pose.position.y,self.source.pose.position.z)) pose = pm.toMsg(pm.fromMatrix(linalg.inv(targetFrameMatrix))*sourceFrame) poseStamped = PoseStamped() poseStamped.pose = pose poseStamped.header.stamp = rospy.get_rostime() poseStamped.header.frame_id = '/'+self.targetLinkName[5:len(self.targetLinkName)] # This strips 'pr2::' from the frame_id string self.wPub.publish(poseStamped)
def pregrasp_object(global_data, object_name, adjust_height, grasp_tran, object_height_col = 2, pregrasp_dist = -.05, object_height_adjustment = 0.0, run_it = True): """@brief - Move the hand to the pregrasp position. This procedure is suprisingly intricate. Given the object filename and the grasp transform, this transforms the grasp to world coordinates then runs a planner to avoid collisions in moving the arm the long distance to the object. This uses the scene graph, which is currently provided by a camera. Returns whether the pregrasp location was reachable, the filename in which the trajectory from OpenRave is stored, and the list of the dof values that are contained in the trajectory. """ """ Sets up object location in openrave -- this code is all to compensate for the current object localization method only knowing the location of the base of the object - the height of the object's origin from the base must be used to adjust it's location in openrave before planning. """ print "Pregrasping object: %s"%(object_name) #Get the object transform in the world. obj_tran = pm.toMatrix(pm.fromTf(global_data.listener.lookupTransform("/world",object_name, rospy.Time(0)))) if (obj_tran == []): warn("Scene graph in TF is broken! Most\ likely, the object transform could not be found.") return [],[], [], [] """Set up and visualize the grasp transforms""" pregrasp_tran = get_pregrasp_tran_from_tran(grasp_tran, pregrasp_dist) publish_grasp_tran(pregrasp_tran) final_tran = dot(obj_tran, pregrasp_tran) publish_grasp_tran(final_tran) success = False """Run the planner""" plan = [] j = [] try: success, trajectory_filename, dof_list, j = run_cbirrt_with_tran( global_data.or_env, final_tran ) except Exception,e: warn("Failed running planner with\ error %s"%e)
def release_gently(global_data, max_dist, ignore_warning=0): """ @brief - Gently place the object on the table by to at most the maximum distance and dropping the object @param global_data - the data structure storing the openrave environment and other global parameters @param max_dist - The highest above the table we allow the arm to be before releasing TODO: Test and debug this function!!!! """ """ This function is not ready yet!""" rospy.log_warn( "release gently function called, but it is not done being developed yet! " ) if not ignore_warning: return success = True reason = "succeeded releasing" #find hand position with respect to world try: hand_in_world = pm.toMatrix( pm.fromTf( listener.lookupTransform('/hand', '/world', rospy.Time(0)))) except: success = False reason = "failed to find hand in world" #Find distance above table and move hand down by that distance if success: try: move_dist = hand_in_world[2, 3] - max_dist if move_dist > 0: lift_arm_to_contact(-move_dist) except: success = False reason = "failed to move arm down" #open the hand to drop the object if success: try: success, reason, position = open_barrett() except: success = False reason = "open barrett command threw exception" return success, reason
def release_gently(global_data, max_dist, ignore_warning = 0): """ @brief - Gently place the object on the table by to at most the maximum distance and dropping the object @param global_data - the data structure storing the openrave environment and other global parameters @param max_dist - The highest above the table we allow the arm to be before releasing TODO: Test and debug this function!!!! """ """ This function is not ready yet!""" rospy.log_warn("release gently function called, but it is not done being developed yet! ") if not ignore_warning: return success = True reason = "succeeded releasing" #find hand position with respect to world try: hand_in_world = pm.toMatrix(pm.fromTf( listener.lookupTransform('/hand','/world', rospy.Time(0)))) except: success = False reason = "failed to find hand in world" #Find distance above table and move hand down by that distance if success: try: move_dist = hand_in_world[2,3] - max_dist if move_dist > 0: lift_arm_to_contact(-move_dist) except: success = False reason = "failed to move arm down" #open the hand to drop the object if success: try: success, reason, position = open_barrett() except: success = False reason = "open barrett command threw exception" return success, reason
def convert_cartesian_world_goal(global_data, world_tran): """@brief - Get armtip goal pose in the arms coordinate system from hand world goal pose. This is useful to convert between a hand goal in the world coordinate system to a cartesian goal that can be sent directly to the staubli controller. @param world_tf - tf of hand goal pose in world coordinates """ try: world_tf = pm.toTf(pm.fromMatrix(world_tran)) bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(world_tf[0], world_tf[1], now, "hand_goal", "world") #listener = tf.TransformListener() global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = global_data.listener.lookupTransform('armbase', 'arm_goal', now) return pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_world_goal failed: error %s.'%e)
def convert_cartesian_relative_goal(global_data, move_tran): """@brief - Convert a position relative to the hand's current coordinate system to the robot's base coordinate system. @param move_tran - the relative position in the hand's coordinate system. """ try: move_tf = pm.toTf(pm.fromMatrix(move_tran)) print move_tf bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(move_tf[0], move_tf[1], now, "hand_goal", "hand") #listener = tf.TransformListener() global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = global_data.listener.lookupTransform('armbase','arm_goal', now) #arm_robot = global_data.listener.lookupTransform('armbase', 'arm', now) return pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_relative_goal failed: error %s.'%e)
def adjust(self, adjust_msg, close_hand = True): """ adjust_msg specifies the adjustment in palm's local coordinate system """ print 'adjust:' print adjust_msg #release the object tp.open_barrett() #tp.move_hand_percentage(0.8) #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(adjust_msg.offset)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1,0,0,0], [0,1,0,0], [0,0,1,-.05], [0,0,0,1]]); back_from_arm_goal = arm_goal * backup #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) #send_cartesian_goal(back_from_arm_goal, True) send_cartesian_goal(arm_goal, True) #raw_input("Press enter to use guarded motion to move forward...") #guarded move forward #GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) #raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #gentlly close the fingers if close_hand: hu.GuardedCloseHand(self.global_data) tp.move_hand_velocity([0.1,0.1,0.1,0])
def call_planner(self, stage): self.status.planning_status = stage print(stage) if stage == ObjectStatus.PREGRASP: target_pose = self.status.pose_stamped.pose pregrasp_distance = .1 #The marker pose is defined in the torso coordinate system target_tran = pm.toMatrix(pm.fromMsg(target_pose)) hand_tran = target_tran.copy() #Align hand perpendicular to cylinder in its canonical pose hand_tran[:3,1] = target_tran[:3,2] hand_tran[:3,2] = target_tran[:3,0] hand_tran[:3,0] = target_tran[:3,1] pregrasp_tran = hand_tran.copy() #The approach direction of this gripper is -y for some reason pregrasp_tran[:3,3] = pregrasp_tran[:3,3] + pregrasp_tran[:3,1]*pregrasp_distance hand_tran_pose = pm.toMsg(pm.fromMatrix(hand_tran)) pregrasp_pose = pm.toMsg(pm.fromMatrix(pregrasp_tran)) req = PosePlanningSrv._request_class() req.TargetPoses = [pregrasp_pose, hand_tran_pose] req.ManipulatorID = int(self.status.hands == self.status.RIGHT) res = None try: print(req) res = self.pregrasp_planner_client.call(req) self.status.operating_status = self.status.PLANNING #print res self.last_plan = res.PlannedTrajectory except: res = None rospy.loginfo("Planner failed to pregrasp") self.status.operating_status = self.status.ERROR
def Rot_k_phi(self, k_vec, phi): #Eigen::Vector3d k_vec,float phi #Eigen::Matrix3d R_k_phi; kx = k_vec[0] ky = k_vec[1] kz = k_vec[2] N = 3 K_nrow = Vector(0.0, -kz, ky) K_trow = Vector(kz, 0.0, -kx) K_brow = Vector(-ky, kx, 0.0) K = Rotation(K_nrow,K_trow,K_brow) I = Rotation() K_Frame = Frame(K) K_Frame_numpy = posemath.toMatrix(K_Frame) K_numpy = K_Frame_numpy[0:3,0:3] I_numpy = np.identity(N) R_k_phi_numpy = I_numpy + math.sin(phi)*K_numpy + (1-math.cos(phi))*K_numpy*K_numpy R_k_phi_numpy_FrameTemp = np.c_[R_k_phi_numpy, np.ones(N)] R_k_phi_numpy_Frame = np.r_[R_k_phi_numpy_FrameTemp,[R_k_phi_numpy_FrameTemp[1]]] R_k_phi_Frame = posemath.fromMatrix(R_k_phi_numpy_Frame) R_k_phi = R_k_phi_Frame.M return R_k_phi
def calib_position(self): ''' Calibrate the suction cup tip location. Pose it as a linear algebra problem @return: ''' A = np.zeros((self.num_pos_measurements()*3, 6)) b = np.zeros((self.num_pos_measurements()*3, 1)) n = 0 for measurement in self._pos_measurements: T = posemath.toMatrix(measurement) R = T[0:3, 0:3] p = T[0:3, 3] A[3*n:3*n+3, 0:3] = R A[3*n:3*n+3, 3:6] = -np.identity(3) b[3*n:3*n+3, 0] = -p n += 1 self._A = A self._b = b sol = np.linalg.lstsq(A, b) x = sol[0] # pose of tool in wrist frame self._F_wrist_tool.p = kdl.Vector(x[0], x[1], x[2]) # position of tool in robot base frame self._F_base_tool.p = kdl.Vector(x[3], x[4], x[5]) return sol
def get_best_distance(self, T, T_fwd, q0, check_closest_only = False, obj_name = None): quaternion_dot = np.dot(np.array(T_fwd.M.GetQuaternion()),np.array(T.M.GetQuaternion())) delta_rotation = np.arccos(2 * quaternion_dot ** 2 - 1) q_new = list() if self.closed_form_IK_solver == None or check_closest_only: q_new.append(self.ik(pm.toMatrix(T),self.q0)) else: q_new = self.closed_form_IK_solver.solveIK(pm.toMatrix(T)) if q_new is not None and len(q_new) > 0: message_print = '' message_print_invalid = '' ik_joint_solution_validity = '' best_dist = float('inf') best_invalid = float('inf') best_q = None best_q_invalid = None for q_i in q_new: if q_i is None: continue dq = np.absolute(q_i - self.q0) * self.joint_weights combined_distance = (T.p - T_fwd.p).Norm() * self.translation_weight + \ self.rotation_weight * delta_rotation + \ self.joint_space_weight * np.sum(dq) # rospy.loginfo(str(q_i)) result = self.check_robot_position_validity(q_i.tolist()) other_obj_collisions = '' if obj_name is not None and not result.valid: other_obj_collision = False contacts = result.contacts for collision in contacts: if collision.contact_body_1 != obj_name and collision.contact_body_2 != obj_name: other_obj_collision = True break # if not other_obj_collision: # rospy.logwarn('[Query] State Pose is actually valid') # rospy.logwarn(str(contacts)) result.valid = not other_obj_collision # ik_joint_solution_validity += '%s ' % result.valid if result.valid and combined_distance < best_dist: best_q = q_i best_dist = combined_distance message_print = 'valid %s pose, translation: %f, rotation: %f, dq6: %f, dist: %f' % \ (obj_name ,(T.p - T_fwd.p).Norm(),self.rotation_weight * delta_rotation, dq[-1], combined_distance ) elif not result.valid and combined_distance < best_invalid: best_q_invalid = q_i best_invalid = combined_distance message_print_invalid = 'invalid %s pose, translation: %f, rotation: %f, dq6: %f, dist: %f' % \ (obj_name, (T.p - T_fwd.p).Norm(),self.rotation_weight * delta_rotation, dq[-1], combined_distance ) message_print_invalid += '\nCollisions found between: ' for contact_info in result.contacts: message_print_invalid += '(%s and %s) '%(contact_info.contact_body_1,contact_info.contact_body_2) if (best_q is not None) or (best_q_invalid is not None): valid_pose = (best_q is not None) if not valid_pose: best_q = best_q_invalid return valid_pose, best_dist, best_invalid, message_print, message_print_invalid, best_q return False, float('inf'), float('inf'), '%s: No valid IK solution'%obj_name,'%s: No valid IK solution'%obj_name, list()
def write_psm2_needle_drive_affines_to_file(self, psm2_gripper_affines_wrt_camera): nposes = len(psm2_gripper_affines_wrt_camera) if (nposes<1): rospy.logwarn("NO POSES TO SAVE") return str1 = "saving computer %d gripper poses w/rt camera"%nposes rospy.loginfo(str1) affine_gripper2_frame_wrt_camera_last = psm2_gripper_affines_wrt_camera[nposes-1] affine_needle_frame_wrt_camera_last = affine_gripper2_frame_wrt_camera_last*self.affine_needle_frame_wrt_gripper_frame_ affine_needle_frame_wrt_camera_last_numpy = posemath.toMatrix(affine_needle_frame_wrt_camera_last) affine_needle_frame_wrt_camera_last_numpy_R = affine_needle_frame_wrt_camera_last_numpy[0:3,0:3] '''BELOW do rest of the calculations with numpy array to prevent data type mismatch and convert to PyKDL at the end''' nvec_needle = affine_needle_frame_wrt_camera_last_numpy_R[0:,0] bvec_needle = affine_needle_frame_wrt_camera_last_numpy_R[0:,2] tvec_needle = affine_needle_frame_wrt_camera_last_numpy_R[0:,1] origin_needle = affine_needle_frame_wrt_camera_last.p origin_needle_numpy = np.array([origin_needle.x(),origin_needle.y(),origin_needle.z()]) tip_of_needle_numpy = origin_needle_numpy + self.needle_radius_*nvec_needle gripper2_grasp_origin_numpy = tip_of_needle_numpy + self.grasp_depth_*bvec_needle gripper2_pre_grasp_origin_numpy = gripper2_grasp_origin_numpy - 0.03*bvec_needle gripper2_out_of_way_numpy = np.array([0.14, -0.03, 0.07]) N = 3 ''' R2_diag_numpy = np.identity(N) R2_diag_numpy[2,2] = -1 R2_diag_numpy[1,1] = -1 print "R2_diag:" print R2_diag_numpy R2_diag_numpy_FrameTemp = np.c_[R2_diag_numpy, np.ones(N)] R2_diag_numpy_Frame = np.r_[R2_diag_numpy_FrameTemp,[R2_diag_numpy_FrameTemp[1]]] R2_diag_Frame = posemath.fromMatrix(R2_diag_numpy_Frame) #BELOW numpy ends here - switch back to PyKDL BELOW R2_diag = R2_diag_Frame.M gripper2_out_of_way = Vector(gripper2_out_of_way_numpy[0],gripper2_out_of_way_numpy[1],gripper2_out_of_way_numpy[2]) psm2_nom_wrt_base2 = Frame(R2_diag,gripper2_out_of_way) print "affine_lcamera_to_psm_two_: " print self.default_affine_lcamera_to_psm_two_.M print "origin: " print self.default_affine_lcamera_to_psm_one_.p psm2_nom_wrt_camera = self.default_affine_lcamera_to_psm_two_*psm2_nom_wrt_base2 print "psm2_nom_wrt_camera: " print psm2_nom_wrt_camera.M print "origin: " print psm2_nom_wrt_camera.p #BELOW switch back to numpy BELOW psm2_nom_wrt_camera_numpy = posemath.toMatrix(psm2_nom_wrt_camera) psm2_nom_wrt_camera_numpy_R = psm2_nom_wrt_camera_numpy[0:3,0:3] ''' tvec_gripper2 = nvec_needle bvec_gripper2 = bvec_needle nvec_gripper2 = np.cross(tvec_gripper2,bvec_gripper2) Rtr = np.array([nvec_gripper2, tvec_gripper2, bvec_gripper2]) print Rtr R = np.transpose(Rtr) print R nvec2 = np.array([1,0,0]) bvec2 = np.array([0,0,1]) t = 4 dt = 1 outfile = open('psm2_gripper_poses_in_camera_coords.csp','w') for i in range(nposes): psm2_gripper_affines_wrt_camera_this = psm2_gripper_affines_wrt_camera[i] #Origin = gripper_affines_wrt_camera_this.p #R = gripper_affines_wrt_camera_this.M gripper_numpy = posemath.toMatrix(psm2_gripper_affines_wrt_camera_this) R = gripper_numpy[0:3,0:3] Origin = gripper_numpy[0:3,3] outfile.write(str("%0.3f"%Origin[0]) + ', ' + str("%0.3f"%Origin[1]) + ', ' + str("%0.3f"%Origin[2]) + ', ') nvec = R[0:,0] tvec = R[0:,1] bvec = R[0:,2] outfile.write(str("%0.3f"%nvec[0]) + ', ' + str("%0.3f"%nvec[1]) + ', ' + str("%0.3f"%nvec[2]) + ', ') outfile.write(str("%0.3f"%bvec[0]) + ', ' + str("%0.3f"%bvec[1]) + ', ' + str("%0.3f"%bvec[2]) + ', 0, ') outfile.write(str("%0.3f"%gripper2_out_of_way_numpy[0]) + ', ' + str("%0.3f"%gripper2_out_of_way_numpy[1]) + ', ' + str("%0.3f"%gripper2_out_of_way_numpy[2]) + ' ') outfile.write(str("%0.3f"%nvec2[0]) + ', ' + str("%0.3f"%nvec2[1]) + ', ' + str("%0.3f"%nvec2[2]) + ', ') outfile.write(str("%0.3f"%bvec2[0]) + ', ' + str("%0.3f"%bvec2[1]) + ', ' + str("%0.3f"%bvec2[2]) + ', 0.0 ' + str("%0.2f"%t) + '\n') t+=dt outfile.close() rospy.loginfo("wrote fripper motion plan to file psm2_gripper_poses_in_camera_coords.csp")
def graspit_grasp_to_moveit_grasp(graspit_grasp_msg, move_group_commander, listener, grasp_tran_frame_name='approach_tran', pregrasp_joint_angles=None, grasp_joint_angles=None): """ :param graspit_grasp_msg: A graspit grasp message :type graspit_grasp_msg: graspit_msgs.msg.Grasp :returns a moveit message built from the graspit grasp :rtype: moveit_msgs.msg.Grasp """ # 'manipulator' is the name for the root move group in the mico arm moveit_positions_from_graspit_positions = {'StaubliArm': barrett_positions_from_graspit_positions, 'manipulator': mico_positions_from_graspit_positions, 'left_arm': lambda x: (['l_gripper_joint'], [x[0]]), 'right_arm': lambda x: (['r_gripper_joint'], [x[0]]) } move_group_name = rospy.get_param('/arm_name', 'StaubliArm') moveit_positions_from_graspit_positions_fcn = moveit_positions_from_graspit_positions[move_group_name] moveit_grasp = moveit_msgs.msg.Grasp() # # This message contains a description of a grasp that would be used # # with a particular end-effector to grasp an object, including how to # # approach it, grip it, etc. This message does not contain any # # information about a "grasp point" (a position ON the object). # # Whatever generates this message should have already combined # # information about grasp points with information about the geometry # # of the end-effector to compute the grasp_pose in this message. # # # A name for this grasp # string id # moveit_grasp.id = 'graspit_' + str(graspit_grasp_msg.grasp_id) # # The internal posture of the hand for the pre-grasp # # only positions are used # # trajectory_msgs/JointTrajectory pre_grasp_posture # pre_grasp_goal_point = trajectory_msgs.msg.JointTrajectoryPoint() if not pregrasp_joint_angles: pregrasp_joint_angles = graspit_grasp_msg.pre_grasp_dof pre_grasp_joint_names, pre_grasp_goal_point.positions = moveit_positions_from_graspit_positions_fcn(pregrasp_joint_angles) moveit_grasp.pre_grasp_posture.points.append(pre_grasp_goal_point) moveit_grasp.pre_grasp_posture.joint_names = pre_grasp_joint_names # # The internal posture of the hand for the grasp # # positions and efforts are used # # trajectory_msgs/JointTrajectory grasp_posture # goal_point = trajectory_msgs.msg.JointTrajectoryPoint() if not grasp_joint_angles: grasp_joint_angles = graspit_grasp_msg.final_grasp_dof joint_names, goal_point.positions = moveit_positions_from_graspit_positions_fcn(grasp_joint_angles) moveit_grasp.grasp_posture.joint_names = joint_names moveit_grasp.grasp_posture.points.append(goal_point) # # The position of the end-effector for the grasp. This is the pose of # # the "parent_link" of the end-effector, not actually the pose of any # # link *in* the end-effector. Typically this would be the pose of the # # most distal wrist link before the hand (end-effector) links began. # # geometry_msgs/PoseStamped grasp_pose # ee_pose = graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg, grasp_tran_frame_name) moveit_grasp.grasp_pose.pose = ee_pose moveit_grasp.grasp_pose.header.frame_id = graspit_grasp_msg.object_name # # The estimated probability of success for this grasp, or some other # # measure of how "good" it is. # # float64 grasp_quality # moveit_grasp.grasp_quality = .8 # # The approach direction to take before picking an object # # GripperTranslation pre_grasp_approach # approach_dir = geometry_msgs.msg.Vector3Stamped() # Set default approach dir to the jaco approach dir. x = rospy.get_param('approach_dir_x', 0) y = rospy.get_param('approach_dir_y', 0) z = rospy.get_param('approach_dir_z', -1) approach_dir.vector = geometry_msgs.msg.Vector3(x, y, z) approach_dir.header.frame_id = grasp_tran_frame_name moveit_grasp.pre_grasp_approach.direction = get_approach_dir_in_ee_coords(move_group_commander, listener, approach_dir) # #Convert the grasp message to a transform grasp_tran = pm.toMatrix(pm.fromMsg(graspit_grasp_msg.final_grasp_pose)) pregrasp_tran = pm.toMatrix(pm.fromMsg(graspit_grasp_msg.pre_grasp_pose)) pregrasp_dist = np.linalg.norm(pregrasp_tran[0:3, 3] - grasp_tran[0:3, 3]) moveit_grasp.pre_grasp_approach.desired_distance = 2*pregrasp_dist moveit_grasp.pre_grasp_approach.min_distance = pregrasp_dist # # The retreat direction to take after a grasp has been completed (object is attached) # # GripperTranslation post_grasp_retreat # moveit_grasp.post_grasp_retreat.min_distance = .05 moveit_grasp.post_grasp_retreat.desired_distance = 2*moveit_grasp.post_grasp_retreat.min_distance moveit_grasp.post_grasp_retreat.direction.header.frame_id = '/world' moveit_grasp.post_grasp_retreat.direction.vector = geometry_msgs.msg.Vector3(0, 0, 1) # # The retreat motion to perform when releasing the object; this information # # is not necessary for the grasp itself, but when releasing the object, # # the information will be necessary. The grasp used to perform a pickup # # is returned as part of the result, so this information is available for # # later use. # # GripperTranslation post_place_retreat # # # the maximum contact force to use while grasping (<=0 to disable) # # float32 max_contact_force # moveit_grasp.max_contact_force = -1 # # an optional list of obstacles that we have semantic information about # # and that can be touched/pushed/moved in the course of grasping # # string[] allowed_touch_objects # moveit_grasp.allowed_touch_objects = [] return moveit_grasp
def Rot_k_phi(self, k_vec, phi): #Eigen::Vector3d k_vec,float phi #Eigen::Matrix3d R_k_phi; kx = k_vec[0] ky = k_vec[1] kz = k_vec[2] #K_nrow = Vector(0.0, -kz, ky) #K_trow = Vector(kz, 0.0, -kx) #K_brow = Vector(-ky, kx, 0.0) N = 3 K = Rotation(0.0,-kz,ky, kz,0.0,-kx, -ky,kx,0.0) Ksquare = K*K I = Rotation() K_Frame = Frame(K) K_Frame_numpy = posemath.toMatrix(K_Frame) K_numpy = K_Frame_numpy[0:3,0:3] I_numpy = np.identity(N) K_square_Frame = Frame(Ksquare) K_square_Frame_numpy = posemath.toMatrix(K_square_Frame) K_square_numpy = K_square_Frame_numpy[0:3,0:3] #R_k_phi_numpy = I_numpy + math.sin(phi)*K_numpy + (1-math.cos(phi))*K_numpy*K_numpy #R_k_phi_numpy_FrameTemp = np.c_[R_k_phi_numpy, np.ones(N)] #R_k_phi_numpy_Frame = np.r_[R_k_phi_numpy_FrameTemp,[R_k_phi_numpy_FrameTemp[1]]] #R_k_phi_Frame = posemath.fromMatrix(R_k_phi_numpy_Frame) #R_k_phi = R_k_phi_Frame.M R_k_phi_numpy_square = I_numpy + math.sin(phi)*K_numpy + (1-math.cos(phi))*K_square_numpy R_k_phi_numpy_FrameTemp_square = np.c_[R_k_phi_numpy_square, np.ones(N)] R_k_phi_numpy_Frame_square = np.r_[R_k_phi_numpy_FrameTemp_square,[R_k_phi_numpy_FrameTemp_square[1]]] R_k_phi_Frame_square = posemath.fromMatrix(R_k_phi_numpy_Frame_square) R_k_phi_square = R_k_phi_Frame_square.M if(debug_needle_print): print "K",K print "Ksquare",Ksquare print "K_Frame",K_Frame print "K_Frame_numpy",K_Frame_numpy print "K_numpy",K_numpy print "K_square_Frame",K_square_Frame print "K_square_Frame_numpy",K_square_Frame_numpy print "math.sin(phi)",(math.sin(phi)) print "math.sin(phi)",(math.sin(phi)) print "1-math.cos(phi)",(1-math.cos(phi)) print "K_numpy*K_numpy",K_numpy*K_numpy print "R_k_phi_numpy",R_k_phi_numpy print "R_k_phi_numpy_FrameTemp",R_k_phi_numpy_FrameTemp print "R_k_phi_numpy_Frame",R_k_phi_numpy_Frame print "R_k_phi_Frame",R_k_phi_Frame print "R_k_phi",R_k_phi print "R_k_phi_numpy_square",R_k_phi_numpy_square print "R_k_phi_numpy_FrameTemp_square",R_k_phi_numpy_FrameTemp_square print "R_k_phi_numpy_Frame_square",R_k_phi_numpy_Frame_square print "R_k_phi_Frame_square",R_k_phi_Frame_square print "R_k_phi_square",R_k_phi_square return R_k_phi_square
def simple_horiz_kvec_motion_psm2(self, O_needle, r_needle, kvec_yaw, gripper_affines_wrt_camera, gripper_affines_wrt_psm): dphi = math.pi/40.0 bvec0 = Vector(1,0,0) nvec = Vector(0,0,1) bvec = self.Rotz(kvec_yaw)*bvec0 tvec = bvec*nvec R0 = Rotation(nvec,tvec,bvec) #R0 = R0.Inverse() tvec_numpy = np.array([tvec.x(), tvec.y(), tvec.z()]) tip_pos_numpy = O_needle - r_needle*tvec_numpy tip_pos = Vector(tip_pos_numpy[0], tip_pos_numpy[1], tip_pos_numpy[2]) self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos self.affine_gripper_frame_wrt_camera_frame_.M = R0 del gripper_affines_wrt_camera[:] nsolns = 0 nphi = 0 print "nphi: " for phi in np.arange(0.0,-math.pi,-dphi): #use arange from numpy for double increments R = self.Rot_k_phi(bvec, phi)*R0 self.affine_gripper_frame_wrt_camera_frame_.M=R R_Frame = posemath.toMatrix(self.affine_gripper_frame_wrt_camera_frame_) R_column2_numpy = R_Frame[0:3,1] R_column2 = Vector(R_column2_numpy[0],R_column2_numpy[1],R_column2_numpy[2]) tip_pos_numpy = O_needle - r_needle*R_column2_numpy tip_pos = Vector(tip_pos_numpy[0], tip_pos_numpy[1], tip_pos_numpy[2]) self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos self.affine_gripper_frame_wrt_psm_frame_ = self.default_affine_lcamera_to_psm_two_.Inverse()*self.affine_gripper_frame_wrt_camera_frame_ affine_gripper_frame_wrt_psm_frame_numpyArray = posemath.toMatrix(self.affine_gripper_frame_wrt_psm_frame_) affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to7 = np.matrix(affine_gripper_frame_wrt_psm_frame_numpyArray) affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6 = affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to7 * inv(frame6to7) affine_0to6 = deepcopy(affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6) #print "affine_0to6", affine_0to6 # qvecIK = deepcopy(qvecIK_Test) #print "qvecIK", qvecIK # result = psm_manip.InverseKinematics(self.qvecIK, affine_0to6) jointLimitCheckBool = self.fit_joints_to_range(self.qvecIK) if result: success = False else: if jointLimitCheckBool: success = True #legal soln else: success = False #no legal soln out of joint lims if (success): affine_camera = deepcopy(self.affine_gripper_frame_wrt_camera_frame_) affine_gripper = deepcopy(self.affine_gripper_frame_wrt_psm_frame_) nsolns+=1 print nphi,"," gripper_affines_wrt_camera.append(affine_camera) gripper_affines_wrt_psm.append(affine_gripper) #gripper_affines_wrt_psm.append(self.affine_gripper_frame_wrt_psm_frame_) ''' ERDEM if (ik_solver_.ik_solve(des_gripper1_wrt_base)) { nsolns++; cout<<nphi<<","; #cout<<": found IK; nsolns = "<<nsolns<<endl; gripper_affines_wrt_camera.push_back(affine_gripper_frame_wrt_camera_frame_); }''' nphi += 1 print "\n"
def simple_horiz_kvec_motion(self, O_needle, r_needle, kvec_yaw, gripper_affines_wrt_camera, gripper_affines_wrt_psm): """ Initialize the IK output joint vector @param self The object @param O_needle The o needle @param r_needle The r needle @param kvec_yaw The kvec yaw @param gripper_affines_wrt_camera The gripper affines wrt camera @param gripper_affines_wrt_psm The gripper affines wrt psm @return { description_of_the_return_value } """ dphi = math.pi/40.0 bvec0 = Vector(1,0,0) nvec = Vector(0,0,-1) bvec = self.Rotz(kvec_yaw)*bvec0 tvec = bvec*nvec R0 = Rotation(nvec,tvec,bvec) #R0 = R0.Inverse() tvec_numpy = np.array([tvec.x(), tvec.y(), tvec.z()]) tip_pos_numpy = O_needle - r_needle*tvec_numpy tip_pos = Vector(tip_pos_numpy[0], tip_pos_numpy[1], tip_pos_numpy[2]) self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos self.affine_gripper_frame_wrt_camera_frame_.M = R0 del gripper_affines_wrt_camera[:] nsolns = 0 nphi = 0 print "nphi: " if debug_needle_print2: print "bvec: ", bvec print "tvec: ", tvec print "tip_pos: ", tip_pos print "R0: ", R0 for phi in np.arange(0,math.pi,dphi): #use arange from numpy for double increments R = self.Rot_k_phi(bvec, phi)*R0 self.affine_gripper_frame_wrt_camera_frame_.M=R R_Frame = posemath.toMatrix(self.affine_gripper_frame_wrt_camera_frame_) R_column2_numpy = R_Frame[0:3,1] R_column2 = Vector(R_column2_numpy[0],R_column2_numpy[1],R_column2_numpy[2]) tip_pos_numpy = O_needle - r_needle*R_column2_numpy tip_pos = Vector(tip_pos_numpy[0], tip_pos_numpy[1], tip_pos_numpy[2]) self.affine_gripper_frame_wrt_camera_frame_.p = tip_pos self.affine_gripper_frame_wrt_psm_frame_ = self.default_affine_lcamera_to_psm_one_.Inverse()*self.affine_gripper_frame_wrt_camera_frame_ # ''' ERDEM # affine_gripper_frame_wrt_psm_frame_numpyArray = posemath.toMatrix(self.affine_gripper_frame_wrt_psm_frame_) affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to7 = np.matrix(affine_gripper_frame_wrt_psm_frame_numpyArray) affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6 = affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to7 * inv(frame6to7) #print "affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6", affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6 affine_0to6 = deepcopy(affine_gripper_frame_wrt_psm_frame_numpyMatrix_0to6) #print "affine_0to6", affine_0to6 #print "qvecIK", qvecIK result = psm_manip.InverseKinematics(self.qvecIK, affine_0to6) jointLimitCheckBool = self.fit_joints_to_range(self.qvecIK) if result: success = False else: if jointLimitCheckBool: #legal soln success = True else: #no legal soln out of joint lims success = False if debug_needle_print2: print "start printing\n" print "\n nphi \n", nphi print "\nR: \n", R print "\ntip_pose: \n", tip_pos print "\nself.affine_gripper_frame_wrt_psm_frame_ \n", self.affine_gripper_frame_wrt_psm_frame_ print "\nself.qvecIK \n", self.qvecIK #print "qvecIK after computation", qvecIK if (success): affine_camera = deepcopy(self.affine_gripper_frame_wrt_camera_frame_) affine_gripper = deepcopy(self.affine_gripper_frame_wrt_psm_frame_) nsolns+=1 print nphi,"," #print "\nqvecIK before computation", qvecIK_before if debug_needle_print2: print "\nqvecIK after computation", self.qvecIK if debug_needle_print3: print "self.affine_gripper_frame_wrt_camera_frame_",affine_camera print "self.affine_gripper_frame_wrt_gripper_frame_",affine_gripper gripper_affines_wrt_camera.append(affine_camera) gripper_affines_wrt_psm.append(affine_gripper) #gripper_affines_wrt_psm.append(self.affine_gripper_frame_wrt_psm_frame_) # ''' ''' gripper_affines_wrt_camera.append(self.affine_gripper_frame_wrt_camera_frame_) gripper_affines_wrt_psm.append(self.affine_gripper_frame_wrt_psm_frame_) ''' nphi += 1 print "\n"
def __init__(self): """ Constructor. This initializes a few data members. """ # data members, event based #needle properties: these will be constant for a given operation print("needle planner constructor: initializations") self.needle_radius_ = DEFAULT_NEEDLE_RADIUS self.needle_axis_ht_ = DEFAULT_NEEDLE_AXIS_HT self.grab_needle_plus_minus_y_ = GRASP_W_NEEDLE_POSITIVE_GRIPPER_Y self.grab_needle_plus_minus_z_ = GRASP_W_NEEDLE_POSITIVE_GRIPPER_Z self.grasp_depth_ = DEFAULT_NEEDLE_GRASP_DEPTH self.phi_grab_ = DEFAULT_PHI_GRAB print "needle arc radius = %f" % self.needle_radius_ print "needle ht above tissue = %f" % self.needle_axis_ht_ b_rt = math.sqrt(self.needle_radius_*self.needle_radius_ - self.needle_axis_ht_*self.needle_axis_ht_) self.dist_entrance_to_exit_ = 2*b_rt print "at needle ht %f, distance from entrance to exit is %f" % (self.needle_axis_ht_, self.dist_entrance_to_exit_) self.bvec_needle_wrt_grasp_frame_ = Vector() self.nvec_needle_wrt_grasp_frame_ = Vector() self.tvec_needle_wrt_grasp_frame_ = Vector() R = Rotation(1,0,0, 0,1,0, 0,0,1) O_grasp_frame = Vector(0,0,-self.grasp_depth_) self.affine_grasp_frame_wrt_gripper_frame_ = Frame(R, O_grasp_frame) #print "NeedlePlanner contructor:" self.O_needle_frame_wrt_grasp_frame_ = Vector() O_needle_ = Vector() self.R_needle_frame_wrt_grasp_frame_ = Rotation() self.affine_needle_frame_wrt_grasp_frame_ = Frame() self.affine_needle_frame_wrt_gripper_frame_ = Frame() self.nvec_tissue_frame_wrt_camera_ = Vector() self.tvec_tissue_frame_wrt_camera_ = Vector() self.bvec_tissue_frame_wrt_camera_ = Vector() self.desired_needle_entrance_point_ = Vector() self.repaired_exit_pt_ = Vector() self.R_tissue_frame_wrt_camera_frame_ = Rotation() self.affine_tissue_frame_wrt_camera_frame_ = Frame() self.O_needle_wrt_tissue_ = Vector(0.5*self.dist_entrance_to_exit_,0,self.needle_axis_ht_) self.bvec_needle_wrt_tissue_frame_ = Vector(0,-1,0) self.nvec_needle_wrt_tissue_frame_ = Vector(-1,0,0) self.tvec_needle_wrt_tissue_frame_ = self.bvec_needle_wrt_tissue_frame_*self.nvec_needle_wrt_tissue_frame_ self.R0_needle_wrt_tissue_ = Rotation(self.nvec_needle_wrt_tissue_frame_,self.tvec_needle_wrt_tissue_frame_,self.bvec_needle_wrt_tissue_frame_) self.affine_init_needle_frame_wrt_tissue_ = Frame(self.R0_needle_wrt_tissue_,self.O_needle_wrt_tissue_) print "INIT: initial affine_needle_frame_wrt_tissue_" print self.affine_init_needle_frame_wrt_tissue_ affine_init_needle_frame_wrt_tissue_numpy_ = posemath.toMatrix(self.affine_init_needle_frame_wrt_tissue_) if(debug_needle_print): print "tvec_needle_wrt_tissue_frame_" print self.tvec_needle_wrt_tissue_frame_ print "self.affine_init_needle_frame_wrt_tissue_" print self.affine_init_needle_frame_wrt_tissue_; print "affine_init_needle_frame_wrt_tissue_numpy_" print affine_init_needle_frame_wrt_tissue_numpy_ print("FIXED: affine_grasp_frame_wrt_gripper_frame_") print self.affine_grasp_frame_wrt_gripper_frame_ self.kvec_needle_ = Vector() self.R_needle_wrt_tissue_ = Rotation() self.affine_needle_frame_wrt_tissue_ = Frame() self.affine_gripper_frame_wrt_tissue_ = Frame() self.affine_needle_frame_wrt_camera_ = Frame() self.affine_gripper_frame_wrt_camera_frame_ = Frame() self.compute_grasp_transform() self.R0_N_wrt_G_= self.affine_needle_frame_wrt_gripper_frame_.M self.O0_N_wrt_G_ = self.affine_needle_frame_wrt_gripper_frame_.p default_p_lcamera_to_psm_one_ = Vector(-0.155,-0.03265,0.0) default_R_lcamera_to_psm_one_ = Rotation(-1,0,0, 0,1,0, 0,0,-1) self.default_affine_lcamera_to_psm_one_ = Frame(default_R_lcamera_to_psm_one_,default_p_lcamera_to_psm_one_) self.default_affine_lcamera_to_psm_two_ = Frame() """ Initialize the IK output joint vector """ self.qvecIK = np.array([0., 0., 0., 0., 0., 0.])