def to_Quaternion(*args, **kwargs): ''' converts the input to a geometry_msgs/Quaternion message supported input: - ROS Pose message (position ignored) - ROS PoseStamped message (position ignored) - ROS Transform message (position ignored) - ROS TransformStamped message (position ignored) - ROS Quaternion message - ROS QuaternionStamped message - kdl Frame (position ignored) - kdl Rotation - tf transform (position ignored) - iterable of length 4 (list, tuple, np.array, ...) (x, y, z, w) - a 3x3 or 4x4 numpy array - individual x, y, z, w values - named x, y, z, w arguments. (minimum of 1 can be specified) - no arguments will result in an identity rotation keyword args: - x, y, z Note: if more than 1 argument then they should be in the order they appear ''' # None passed in if (len(args) == 1 and args[0] == None): return geometry_msgs.Quaternion(0, 0, 0, 1) if (len(args) == 1): # Quaternion if (type(args[0]) == geometry_msgs.Quaternion): return copy.deepcopy(args[0]) # QuaternionStamped if (type(args[0]) == geometry_msgs.QuaternionStamped): return copy.deepcopy(args[0].quaternion) # Pose if (type(args[0]) == geometry_msgs.Pose): return copy.deepcopy(args[0].orientation) # PoseStamped if (type(args[0]) == geometry_msgs.PoseStamped): return copy.deepcopy(args[0].pose.orientation) # Transform if (type(args[0]) == geometry_msgs.Transform): return copy.deepcopy(args[0].rotation) # TransformStamped if (type(args[0]) == geometry_msgs.TransformStamped): return copy.deepcopy(args[0].transform.rotation) # kdl Frame if (type(args[0]) == kdl.Frame): return geometry_msgs.Quaternion(*args[0].M.GetQuaternion()) # kdl Rotation if (type(args[0]) == kdl.Rotation): return geometry_msgs.Quaternion(*args[0].GetQuaternion()) # 3x3 or 4x4 numpy array if (type(args[0]) == np.ndarray and args[0].ndim == 2): (r, c) = args[0].shape if (r >= 3 and c >= 3): rot = kdl.Rotation(args[0][0, 0], args[0][0, 1], args[0][0, 2], args[0][1, 0], args[0][1, 1], args[0][1, 2], args[0][2, 0], args[0][2, 1], args[0][2, 2]) return geometry_msgs.Quaternion(*rot.GetQuaternion()) # tf transform ((x,y,z), (x,y,z,w)) if (hasattr(args[0], '__iter__') and len(args[0]) == 2 and len(args[0][1]) == 4): return geometry_msgs.Quaternion(*args[0][1][:4]) # iterable length 4 if (hasattr(args[0], '__iter__') and len(args[0]) >= 4): return geometry_msgs.Quaternion(*args[0][:4]) # 4 arguments elif (len(args) == 4): return geometry_msgs.Quaternion(*args) # x, y, z, or w keyword arguments elif ('x' in kwargs or 'y' in kwargs or 'z' in kwargs or 'w' in kwargs): x = 0.0 y = 0.0 z = 0.0 w = 0.0 if ('x' in kwargs): x = kwargs['x'] if ('y' in kwargs): y = kwargs['y'] if ('z' in kwargs): z = kwargs['z'] if ('w' in kwargs): w = kwargs['w'] return geometry_msgs.Quaternion(x, y, z, w) elif (len(args) == 0): return geometry_msgs.Quaternion(0.0, 0.0, 0.0, 1.0)
import numpy as np import PyKDL as kdl # When taking the rotation vector directly from KDL, there are # numeric problems when the rotation matrix is diagonal, for example # 180 degree rotation around any or the axes. In this case the vector # is set to zero even if it should be for example (0, 0, pi) for a # 180 degree rotation around z # frames.cpp line 325 GetRot rot = kdl.Rotation() rot.DoRotZ(np.pi) print rot.GetRot() rot = kdl.Rotation() rot.DoRotZ(np.pi + 0.001) print rot.GetRot() rot3 = kdl.Rotation() rot3.DoRotX(np.pi) print rot3.GetRotAngle() rot2 = kdl.Rotation() rot2.DoRotY(np.pi) print rot2.GetRotAngle() rot4 = kdl.Rotation() rot4.DoRotZ(np.pi) print rot4.GetRotAngle()
def step(self, dt, lin_vel=(0, 0, 0), rot_vel=(0, 0, 0), check=False, base_time=None): q0 = self._q X0 = self._x dist = np.linalg.norm(lin_vel) if dist > 0: lin_step = kdl.Vector(*(np.array(lin_vel) * dt)) else: lin_step = kdl.Vector() angle = np.linalg.norm(rot_vel) if angle > 0: axis = np.array(rot_vel) / angle rot_step = kdl.Rotation.Rot(kdl.Vector(*axis), angle * dt) else: rot_step = kdl.Rotation() base_tf = kdl.Frame(kdl.Rotation(), lin_step) tool_tf = kdl.Frame(rot_step, kdl.Vector()) X1 = base_tf * X0 * tool_tf X2 = base_tf * X1 * tool_tf p = np.array([X1.p.x(), X1.p.y(), X1.p.z()]) if np.any(p < self._workspace[0, :] - 0.01) or np.any( p > self._workspace[1, :] + 0.01): # raise ControllerException( # 'Target outside workspace bounds: {} / {}'.format(p, self._workspace)) return p = np.array([X2.p.x(), X2.p.y(), X2.p.z()]) if np.any(p < self._workspace[0, :] - 0.01) or np.any( p > self._workspace[1, :] + 0.01): # raise ControllerException( # 'Target outside workspace bounds: {} / {}'.format(p, self._workspace)) return q1 = self._kin.inverse(pm.toMatrix(X1), q_guess=q0) if q1 is None: raise ControllerException('IK solution not found') jerk = np.max(np.abs(np.subtract(q0, q1))) if jerk > 0.2: raise ControllerException('IK solution not continuous') q2 = self._kin.inverse(pm.toMatrix(X2), q_guess=q1) if q2 is None: raise ControllerException('IK solution not found') jerk = np.max(np.abs(np.subtract(q1, q2))) if jerk > 0.2: raise ControllerException('IK solution not continuous') if check: points = [pm.toMsg(w) for w in [X1, X2]] (_, frac) = self._group.compute_cartesian_path(points, 0.01, 5.0) if frac < 1.0: raise ControllerException('Moveit planner failed') self._q = q1 self._x = X1 traj = JointTrajectory() if base_time is not None: traj.header.stamp = base_time traj.joint_names = self._joint_names # target position point = JointTrajectoryPoint() point.positions = q1 point.time_from_start = rospy.Duration(dt) traj.points.append(point) # prediction for future on case huge delays point = JointTrajectoryPoint() point.positions = q2 point.time_from_start = rospy.Duration(dt * 2) traj.points.append(point) self._publisher.publish(traj)
def add_mat(mat1, mat2): out = PyKDL.Rotation() for i in range(0, 3): for j in range(0, 3): out[i, j] = mat1[i, j] + mat2[i, j] return out
def skew_mat(v): return PyKDL.Rotation(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0)
def test_initialize2(self): self.robot.base.get_location = lambda: FrameStamped( kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(20, 20, 2)), "/map" ) self.tour_guide.initialize() self.assertListEqual(self.tour_guide._passed_room_ids, [])
def setUp(self): self.robot.base.get_location = lambda: FrameStamped( kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(-1, -1, 0)), "/map" ) self.tour_guide.initialize()
def np_rot_to_kdl(rot): return kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2])
kdl_pose = psm2.get_current_position() print("Current Position:") pprint.pprint(kdl_pose) """ An arbitrary z starting point that is not too close/far from the platform. When the gripper picks up a needle, it moves up to this point and then releases the needle. Change if it is too high/low above the platform. """ z_upper = -0.08 # z_final = -0.06 """ Where PSM2 touches the platform """ # For white background: z_lower = -0.1233 # For phantom background: # z_lower = -0.128 """ POSE PSM2 WAS CALIBRATED IN """ pos = PyKDL.Vector(-0.118749, 0.0203151, -0.081688) rot = PyKDL.Rotation(-0.988883, -0.00205771, -0.148682, -0.00509171, 0.999786, 0.0200282, 0.148609, 0.0205626, -0.988682) pos2 = PyKDL.Vector(-0.0972128, -0.0170138, -0.106974) sideways = PyKDL.Rotation(-0.453413, 0.428549, -0.781513, -0.17203, 0.818259, 0.548505, 0.874541, 0.383143, -0.297286) """ Move to arbitrary start position (near upper left corner) & release anything gripper is holding. """ # home(psm2, pos, rot) home(psm2, pos2, sideways) """ Get PSM and endoscope calibration data (25 corresponding chess points) """ psm2_calibration_data = list( transform.load_all('../utils/psm2_recordings.txt')) psm2_calibration_matrix = transform.psm_data_to_matrix( psm2_calibration_data) endoscope_calibration_matrix = np.matrix(
def displayRegistration(cams, camModel, toolOffset, camTransform, tfSync, started=False, affine=None, targetLink=None): rate = rospy.Rate(15) # 15hz while not rospy.is_shutdown(): # Get last images imageL = cams.camL.image imageR = cams.camR.image # Wait for images to exist if type(imageR) == type(None) or type(imageL) == type(None): rate.sleep() continue # Check we have valid images (rows, cols, channels) = imageL.shape if cols < 60 or rows < 60 or imageL.shape != imageR.shape: rate.sleep() continue # Get position if targetLink is None: poseMsg = tfSync.synchedMessages[0].pose robotPosition = posemath.fromMsg(poseMsg) else: transforms = tfSync.getTransforms([targetLink], tfSync.frames[0]) pos = (transforms[0].transform.translation.x, transforms[0].transform.translation.y, transforms[0].transform.translation.z) rot = (transforms[0].transform.rotation.x, transforms[0].transform.rotation.y, transforms[0].transform.rotation.z, transforms[0].transform.rotation.w) robotPosition = posemath.fromTf((pos, rot)) # Find 3D position of end effector offset = getOffset(robotPosition, toolOffset) pVector = robotPosition.p pos = np.matrix([pVector.x(), pVector.y(), pVector.z()]) + offset pos = np.vstack((pos.transpose(), [1])) # Publish TFs for easier debugging pubTF(robotPosition, tfSync.frames[0], 'get_position') pubTF( PyKDL.Frame(PyKDL.Rotation(), PyKDL.Vector(pos[0], pos[1], pos[2])), tfSync.frames[0], 'tip_pose') # Calculate 3D point in camera space if affine is None: pos = np.linalg.inv(camTransform) * pos else: pos = np.linalg.inv(affine * camTransform) * pos # Project position into 2d coordinates posL = camModel.left.project3dToPixel(pos) posL = [int(l) for l in posL] posR = camModel.right.project3dToPixel(pos) posR = [int(l) for l in posR] (rows, cols) = imageL.shape[0:2] posR = (posR[0] + cols, posR[1]) transforms = tfSync.getTransforms() posEnd = posL for i in range(0, len(transforms) - 1): start = [ transforms[i].transform.translation.x, transforms[i].transform.translation.y, transforms[i].transform.translation.z ] end = [ transforms[i + 1].transform.translation.x, transforms[i + 1].transform.translation.y, transforms[i + 1].transform.translation.z ] # Project position into 2d coordinates posStartL = camModel.left.project3dToPixel(start) posEndL = camModel.left.project3dToPixel(end) posStartR = camModel.right.project3dToPixel(start) posEndR = camModel.right.project3dToPixel(end) # Draw on left and right images if not np.isnan(posStartL + posEndL + posStartR + posEndR).any(): posStartL = [int(l) for l in posStartL] posEndL = [int(l) for l in posEndL] cv2.line(imageL, tuple(posStartL), tuple(posEndL), (0, 255, 0), 1) posStartR = [int(l) for l in posStartR] posEndR = [int(l) for l in posEndR] cv2.line(imageR, tuple(posStartR), tuple(posEndR), (0, 255, 0), 1) cv2.line(imageL, tuple(posEnd), tuple(posL), (0, 255, 0), 1) point3d, image = calculate3DPoint(imageL, imageR, camModel) # Draw images and display them cv2.circle(image, tuple(posL), 2, (255, 255, 0), -1) cv2.circle(image, tuple(posR), 2, (255, 255, 0), -1) cv2.circle(image, tuple(posL), 7, (255, 255, 0), 2) cv2.circle(image, tuple(posR), 7, (255, 255, 0), 2) if not started: message = "Press s to start registration. Robot will move to its joint limits." cv2.putText(image, message, (50, 50), cv2.FONT_HERSHEY_DUPLEX, 1, [0, 0, 255]) message = "MAKE SURE AREA IS CLEAR" cv2.putText(image, message, (50, 100), cv2.FONT_HERSHEY_DUPLEX, 1, [0, 0, 255]) cv2.imshow(_WINDOW_NAME, image) key = cv2.waitKey(1) if key == 27: cv2.destroyAllWindows() quit() # esc to quit elif not started and (chr(key % 256) == 's' or chr(key % 256) == 'S'): break # s to continue rate.sleep()
fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(arm_chain) Jnt_list = PyKDL.JntArray(7) Jnt_list_dmp = PyKDL.JntArray(7) pose = [] pose_dmp = [] for i in range(train_len): Jnt_list[0] = joint0_data[i] Jnt_list[1] = joint1_data[i] Jnt_list[2] = joint2_data[i] Jnt_list[3] = joint3_data[i] Jnt_list[4] = joint4_data[i] Jnt_list[5] = joint5_data[i] Jnt_list[6] = joint6_data[i] fk_p_kdl.JntToCart(Jnt_list, end_frame) pos = end_frame.p rot = PyKDL.Rotation(end_frame.M) rot = rot.GetQuaternion() pose.append( np.array([pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]])) xs = [] ys = [] zs = [] for j in range(train_len): position_x = pose[j][0] position_y = pose[j][1] position_z = pose[j][2] xs.append(position_x) ys.append(position_y) zs.append(position_z)
def __init__(self, vol_radius, vol_samples_count, T_H_O, orientations_angle, vertices_obj, faces_obj): self.vol_radius = vol_radius self.vol_samples_count = vol_samples_count self.index_factor = float(self.vol_samples_count)/(2.0*self.vol_radius) self.vol_samples = [] for x in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples.append([]) for y in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples[-1].append([]) for z in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples[-1][-1].append([]) self.vol_samples[-1][-1][-1] = {} self.vol_sample_points = [] for xi in range(self.vol_samples_count): for yi in range(self.vol_samples_count): for zi in range(self.vol_samples_count): self.vol_sample_points.append( self.getVolPoint(xi,yi,zi) ) self.T_H_O = T_H_O self.T_O_H = self.T_H_O.Inverse() # generate the set of orientations self.orientations_angle = orientations_angle normals_sphere = velmautils.generateNormalsSphere(self.orientations_angle) orientations1 = velmautils.generateFramesForNormals(self.orientations_angle, normals_sphere) orientations2 = [] for ori in orientations1: x_axis = ori * PyKDL.Vector(1,0,0) if x_axis.z() > 0.0: orientations2.append(ori) self.orientations = {} for ori_idx in range(len(orientations2)): self.orientations[ori_idx] = orientations2[ori_idx] # generate a set of surface points self.surface_points_obj = surfaceutils.sampleMeshDetailedRays(vertices_obj, faces_obj, 0.0015) print "surface of the object has %s points"%(len(self.surface_points_obj)) # disallow contact with the surface points beyond the key handle for p in self.surface_points_obj: if p.pos.x() > 0.0: p.allowed = False surface_points_obj_init = [] surface_points2_obj_init = [] for sp in self.surface_points_obj: if sp.allowed: surface_points_obj_init.append(sp) else: surface_points2_obj_init.append(sp) print "generating a subset of surface points of the object..." while True: p_idx = random.randint(0, len(self.surface_points_obj)-1) if self.surface_points_obj[p_idx].allowed: break p_dist = 0.003 self.sampled_points_obj = [] while True: self.sampled_points_obj.append(p_idx) surface_points_obj2 = [] for sp in surface_points_obj_init: if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist: surface_points_obj2.append(sp) if len(surface_points_obj2) == 0: break surface_points_obj_init = surface_points_obj2 p_idx = surface_points_obj_init[0].id print "subset size: %s"%(len(self.sampled_points_obj)) print "generating a subset of other surface points of the object..." p_dist2 = 0.006 while True: p_idx = random.randint(0, len(self.surface_points_obj)-1) if not self.surface_points_obj[p_idx].allowed: break self.sampled_points2_obj = [] while True: self.sampled_points2_obj.append(p_idx) surface_points2_obj2 = [] for sp in surface_points2_obj_init: if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist2: surface_points2_obj2.append(sp) if len(surface_points2_obj2) == 0: break surface_points2_obj_init = surface_points2_obj2 p_idx = surface_points2_obj_init[0].id # test volumetric model if False: for pt_idx in self.sampled_points2_obj: pt = self.surface_points_obj[pt_idx] m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None) rospy.sleep(0.001) for pt_idx in self.sampled_points_obj: pt = self.surface_points_obj[pt_idx] m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None) rospy.sleep(0.001) print "subset size: %s"%(len(self.sampled_points2_obj)) print "calculating surface curvature at sampled points of the obj..." m_id = 0 planes = 0 edges = 0 points = 0 for pt_idx in range(len(self.surface_points_obj)): indices, nx, pc1, pc2 = surfaceutils.pclPrincipalCurvaturesEstimation(self.surface_points_obj, pt_idx, 5, 0.003) # m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + nx*0.004), m_id, 1, 0, 0, frame='world', namespace='default', scale=0.0002) # m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + self.surface_points_obj[pt_idx].normal*0.004), m_id, 0, 0, 1, frame='world', namespace='default', scale=0.0002) self.surface_points_obj[pt_idx].frame = PyKDL.Frame(PyKDL.Rotation(nx, self.surface_points_obj[pt_idx].normal * nx, self.surface_points_obj[pt_idx].normal), self.surface_points_obj[pt_idx].pos) self.surface_points_obj[pt_idx].pc1 = pc1 self.surface_points_obj[pt_idx].pc2 = pc2 if pc1 < 0.2: self.surface_points_obj[pt_idx].is_plane = True self.surface_points_obj[pt_idx].is_edge = False self.surface_points_obj[pt_idx].is_point = False planes += 1 elif pc2 < 0.2: self.surface_points_obj[pt_idx].is_plane = False self.surface_points_obj[pt_idx].is_edge = True self.surface_points_obj[pt_idx].is_point = False edges += 1 else: self.surface_points_obj[pt_idx].is_plane = False self.surface_points_obj[pt_idx].is_edge = False self.surface_points_obj[pt_idx].is_point = True points += 1 print "obj planes: %s edges: %s points: %s"%(planes, edges, points)
import rospy import dvrk import numpy as np import signal import PyKDL from sensor_msgs.msg import Joy def get_cartesian(pose): position = pose.p x = position.x() y = position.y() z = position.z() output = np.array([x, y, z]) return output """define the waypoint positions for the PSMs for this load test""" cart1 = PyKDL.Vector(0.14514, -0.0666, -0.09294) rot1 = PyKDL.Rotation() rot1 = rot1.Quaternion(0.7209, -0.0192, 0.6925, 0.0152) pos1 = PyKDL.Frame(rot1, cart1) p2 = dvrk.psm('PSM2') # set our rate to 30hz rate = rospy.Rate(30) print 'homing to position...' p2.move(pos1)
print(Xb) z = Xa*Xb print(z) y = Xa*z print(y) z_norm = z.Normalize() y_norm = y.Normalize() Xa_norm = Xa.Normalize() print("X normalized: " + str(Xa)) print("Y normalized: " + str(y)) print("Z normalized: " + str(z)) #vectors = numpy.array([[xyz1.p[0],xyz1.p[1],xyz1.p[2],Xa[0],Xa[1],Xa[2]], # [xyz1.p[0],xyz1.p[1],xyz1.p[2],y[0],y[1],y[2]], # [xyz1.p[0],xyz1.p[1],xyz1.p[2],z[0],z[1],z[2]]]) #X,Y,Z,U,V,W = zip(*vectors) #fig=plt.figure() #ax = fig.add_subplot(111,projection='3d') #ax.quiver(X,Y,Z,U,V,W) #plt.show() ori_v = PyKDL.Vector(xyz1.p[0],xyz1.p[1],xyz1.p[2]) ori_r = PyKDL.Rotation(Xa[0],y[0],z[0],Xa[1],y[1],z[1],Xa[2],y[2],z[2]) surgical_cs = PyKDL.Frame(ori_r,ori_v) #ori_xyz_c = numpy.array( [[ Xa[0], y[0], z[0]] ,[Xa[1], y[1] ,z[1]], [Xa[2], y[2] ,z[2]]]) print(surgical_cs) #print(ori_xyz_c)
def __init__(self, base=PyKDL.Frame(), size=np.array([1, 1, 1]), size_m=np.array([0.05, 0.05, 0.05]), indices_offset=np.array([0, 0, 1]), relative_frames=True, execution_order=[0, 1, 2], name_matrix=""): super(TfMatrixSphere, self).__init__(base=base, size=size, size_m=size_m) a_lb = range(0, self.size[0]) b_lb = range(0, self.size[1]) c_lb = range(0, self.size[2]) n = self.size[0] * self.size[1] * self.size[2] c_list = c_lb a_list = [] b_list = [] for b in range(0, self.size[1] * self.size[2]): a_list.extend(a_lb) a_lb.reverse() for c in range(0, self.size[2]): b_list.extend(b_lb) b_lb.reverse() indices_list = [] for s in range(0, n): a = s b = int(s / self.size[0]) c = int(s / (self.size[0] * self.size[1])) index = (a_list[a], b_list[b], c_list[c]) indices_list.append(index) for index in indices_list: ithetaZ = index[execution_order[0]] ithetaY = index[execution_order[1]] iz = index[execution_order[2]] dthetaZ = size_m[0] * (ithetaZ + 1 + indices_offset[0]) dthetaY = size_m[1] * (ithetaY + 1 + indices_offset[1]) dz = size_m[2] * (iz + 1 + indices_offset[2]) if name_matrix != "": name_matrix = "_" + name_matrix else: name = "" current_name = "matrix_sphere_{}_{}_{}{}".format( ithetaZ, ithetaY, iz, name) Tr_rz = PyKDL.Frame(PyKDL.Rotation().RotZ(dthetaZ)) Tr_ry = PyKDL.Frame(PyKDL.Rotation().RotY(dthetaY)) Tr_z = PyKDL.Frame(PyKDL.Vector(0, 0, dz)) Tr = Tr_rz * Tr_ry * Tr_z if relative_frames: current_frame = self.base * Tr else: current_frame = self.base + Tr self.frames_names.append(current_name) self.frames_map[current_name] = current_frame
for v in eigVectors: m = markerVector(id, v * eigValues[id - 1], mean) id = id + 1 points_pub.publish(m) #building the rotation matrix eigx_n = PyKDL.Vector(Eigenvectors[0, 0], Eigenvectors[0, 1], Eigenvectors[0, 2]) eigy_n = -PyKDL.Vector(Eigenvectors[1, 0], Eigenvectors[1, 1], Eigenvectors[1, 2]) eigz_n = PyKDL.Vector(Eigenvectors[2, 0], Eigenvectors[2, 1], Eigenvectors[2, 2]) eigx_n.Normalize() eigy_n.Normalize() eigz_n.Normalize() rot = PyKDL.Rotation(eigx_n, eigy_n, eigz_n) quat = rot.GetQuaternion() rospy.loginfo(quat) rospy.loginfo(Eigenvectors[0, 2]) #painting the Gaussian Ellipsoid Marker marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = eigValues[0] * 2 marker.scale.y = eigValues[1] * 2 marker.scale.z = eigValues[2] * 2 marker.color.a = 0.5 marker.color.r = 0.0
import math # ROS import PyKDL as kdl # TU/e Robotics from robot_skills.util.kdl_conversions import FrameStamped from robocup_knowledge import knowledge_loader # Common knowledge common = knowledge_loader.load_knowledge("common") # Detection cabinet_amcl = "bookcase" cabinet_poses = [ FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(0.144, 3.574, 0.0)), frame_id="map"), FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 1.570796), kdl.Vector(2.271, -1.258, 0.0)), frame_id="map") ] object_shelves = ["shelf2", "shelf3", "shelf4", "shelf5"] object_types = [obj["name"] for obj in common.objects] # Grasping grasp_surface = "cabinet" room = "dining_room" # Placing default_place_entity = cabinet_amcl
#from Roll Pitch, Yaw angles to a rotation rpy = r1.GetRPY() print "rpy:", rpy # create a rotation from XYZ Euler angles r2 = PyKDL.Rotation.EulerZYX(0, 1, 0) print"r2:\n", r2 #from XYZ Euler angles to a rotation EulerZYX = r2.GetEulerZYX() print "EulerZYX:", EulerZYX print "Quaternion:", r2.GetQuaternion() # create a rotation from a rotation matrix r3 = PyKDL.Rotation(1, 0, 0, 2, 1, 0, 3, 0, 1) print "r3:\n", r3 #create a rotation from a Quaternion r4 = PyKDL.Rotation.Quaternion(0.0, 0.47942553860420295, 0.0, 0.8775825618903728) print "r4:\n", r4 #from a rotation to a Quaternion Quaternion = r4.GetEulerZYX() print "Quaternion:", Quaternion #create a RotAngle from a rotation [rot_angle, rot_vector] = r4.GetRotAngle() print "rot_angle:", rot_angle print "rot_vector:", rot_vector
def test_describe_near_objects2(self): self.robot.base.get_location = lambda: FrameStamped( kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(6, 4, 0)), "/map") self.assertEqual("We now enter the bedroom", self.tour_guide.describe_near_objects())
def np_array2PyKDL_Rotation(np_array): return PyKDL.Rotation(np_array[0,0], np_array[0,1], np_array[0,2], np_array[1,0], np_array[1,1], np_array[1,2], np_array[2,0], np_array[2,1], np_array[2,2])
def arrayToPyKDLRotation(array): x = PyKDL.Vector(array[0][0], array[1][0], array[2][0]) y = PyKDL.Vector(array[0][1], array[1][1], array[2][1]) z = PyKDL.Vector(array[0][2], array[1][2], array[2][2]) return PyKDL.Rotation(x, y, z)
def limbPose(kdl_tree, base_link, limb_interface, limb='right'): tip_link = limb + '_gripper' tip_frame = PyKDL.Frame() arm_chain = kdl_tree.getChain(base_link, tip_link) # Baxter Interface Limb Instances #limb_interface = baxter_interface.Limb(limb) joint_names = limb_interface.joint_names() num_jnts = len(joint_names) if limb == 'right': limb_link = [ 'base', 'torso', 'right_arm_mount', 'right_upper_shoulder', 'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm', 'right_wrist', 'right_hand', 'right_gripper_base', 'right_gripper' ] else: limb_link = [ 'base', 'torso', 'left_arm_mount', 'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm', 'left_wrist', 'left_hand', 'left_gripper_base', 'left_gripper' ] limb_frame = [] limb_chain = [] limb_pose = [] limb_fk = [] for idx in xrange(arm_chain.getNrOfSegments()): linkname = limb_link[idx] limb_frame.append(PyKDL.Frame()) limb_chain.append(kdl_tree.getChain(base_link, linkname)) limb_fk.append( PyKDL.ChainFkSolverPos_recursive( kdl_tree.getChain(base_link, linkname))) # get the joint positions cur_type_values = limb_interface.joint_angles() while len(cur_type_values) != 7: print "Joint angles error!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" cur_type_values = limb_interface.joint_angles() kdl_array = PyKDL.JntArray(num_jnts) for idx, name in enumerate(joint_names): kdl_array[idx] = cur_type_values[name] limb_joint = [ PyKDL.JntArray(1), PyKDL.JntArray(2), PyKDL.JntArray(3), PyKDL.JntArray(4), PyKDL.JntArray(5), PyKDL.JntArray(6), PyKDL.JntArray(7) ] for i in range(7): for j in range(i + 1): limb_joint[i][j] = kdl_array[j] for i in range(arm_chain.getNrOfSegments()): joint_array = limb_joint[limb_chain[i].getNrOfJoints() - 1] limb_fk[i].JntToCart(joint_array, limb_frame[i]) pos = limb_frame[i].p rot = PyKDL.Rotation(limb_frame[i].M) rot = rot.GetQuaternion() limb_pose.append([pos[0], pos[1], pos[2]]) return np.asarray(limb_pose), kdl_array
def scalar_mul(mat, s): out = PyKDL.Rotation() for i in range(0, 3): for j in range(0, 3): out[i, j] = mat[i, j] * s return out
if __name__ == '__main__': psm2 = robot.robot('PSM2') kdl_pose = psm2.get_current_position().p print("Current Position:") pprint.pprint(kdl_pose) pprint.pprint(psm2.get_current_position().M) """ An arbitrary z starting point that is not too close/far from the platform. When the gripper picks up a needle, it moves up to this point and then releases the needle. Change if it is too high/low above the platform. """ z_upper = -0.112688 """ POSE PSM2 WAS CALIBRATED IN """ pos = PyKDL.Vector(-0.118749, 0.0203151, -0.111688) rot = PyKDL.Rotation(-0.940475, 0.313929, -0.130214, 0.330906, 0.933198, -0.140157, 0.0775163, -0.174902, -0.98153) pos2 = PyKDL.Vector(-0.0972128, -0.0170138, -0.106974) sideways = PyKDL.Rotation(-0.453413, 0.428549, -0.781513, -0.17203, 0.818259, 0.548505, 0.874541, 0.383143, -0.297286) """ Move to arbitrary start position (near upper left corner) & release anything gripper is holding. """ home(psm2, pos2, sideways) """ Get PSM and endoscope calibration data (25 corresponding chess points) """ psm2_calibration_data = list( transform.load_all('utils/psm2_recordings.txt')) psm2_calibration_matrix = transform.fit_to_plane( transform.psm_data_to_matrix(psm2_calibration_data)) endoscope_calibration_matrix = transform.fit_to_plane( np.matrix(
def main(): rospy.init_node('controller', anonymous=True) robot = psm('PSM1') rate = rospy.Rate(100) # 10hz robot.home() position_start = robot.get_current_position() safe_pos = PyKDL.Frame( PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0), PyKDL.Vector(0, 0, -1)), PyKDL.Vector(0.02, -0.02, -0.09)) robot.move(safe_pos) # just some stuff to aid prototyping and visualization # has nothing to do with dVRK controls fig = plt.figure() ax = fig.gca(projection='3d') coefs = (100, 200, 100 ) # Coefficients in a0/c x**2 + a1/c y**2 + a2/c z**2 = 1 # Radii corresponding to the coefficients: rx, ry, rz = 1 / np.sqrt(coefs) # Set of all spherical angles: u = np.linspace(0, 2 * np.pi, 100) v = np.linspace(0, 2 * np.pi, 100) #v = np.linspace(0, np.pi, 100) # Cartesian coordinates that correspond to the spherical angles: # (this is the equation of an ellipsoid): # https://stackoverflow.com/questions/7819498/plotting-ellipsoid-with-matplotlib x = rx * np.outer(np.cos(u), np.sin(v)) y = ry * np.outer(np.sin(u), np.sin(v)) z = rz * np.outer(np.ones_like(u), np.cos(v)) # print(np.amax(z)) z_max = np.amax(z) # retain just the positive half of ellipsoid z[z < 0] = 0 # ensure it sits on the xy plane z = z - z_max # Plot: ax.plot_wireframe(x, y, z, rstride=4, cstride=4, color='r', edgecolor='None') ax.scatter3D(x, y, z, c='r', s=5, zorder=10) # Adjustment of the axes, so that they all have the same span: max_radius = max(rx, ry, rz) for axis in 'xyz': getattr(ax, 'set_{}lim'.format(axis))((-max_radius, max_radius)) ## random points on surface xc = rx * np.outer(np.cos(u), np.sin(v)) yc = ry * np.outer(np.sin(u), np.sin(v)) zc = rz * np.outer(np.ones_like(u), np.cos(v)) zc[zc < 0] = 0 zc = zc - z_max i = np.random.choice(5000, 100) xr = np.ravel(xc)[i] yr = np.ravel(yc)[i] zr = np.ravel(zc)[i] ax.scatter(xr, yr, zr, s=50, c='b', zorder=10) plt.show() # CODE TO SCAN # dVRK controls begin here # make 3D points segmented_points = np.vstack( (xr, yr, zr)).T # should be in robot's coordinates # width of the liver = 8" = 20 cm # length of the liver = 4" = 10 cm startPos_x = np.amax(xr) frontPos_y = np.amax(yr) rearPos_y = np.amin(yr) endPos_x = np.amin(xr) # xr, yr, zr must be in METERS # Adjustments in positions and heights will have to be made before running on dVRK startPos = PyKDL.Frame( PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0), PyKDL.Vector(0, 0, -1)), PyKDL.Vector(startPos_x, rearPos_y, -0.08)) # These step sizes below are calculated based on the width and length of the liver # will have to tune when run on the dVRK step_val_x = 0.004 step_val_y = 0.001 step_val_z = 0.0001 dir = 1 z_dir = 1 x_iter = int((startPos_x - endPos_x) / step_val_x) y_iter = int( (frontPos_y - rearPos_y) / step_val_y ) # we can change from where it needs to start scanning in the y-coordinate for i in range(x_iter): # [width/step_val_x] for j in range( y_iter - 75 ): # [len/step_val_y] # will have to tune these values on dVRK if j > (y_iter / 2): z_dir = -1 robot.dmove(PyKDL.Vector(0, step_val_y * dir, step_val_z * z_dir)) robot.dmove(PyKDL.Vector(-step_val_x, 0, 0)) dir *= -1 z_dir = 1 while not rospy.is_shutdown(): print("Scan Complete") rate.sleep()
def predict(self, img, box_3d_color, box_3d_color1): ori_img = img img = cv2.resize(img,(self.test_width,self.test_height)) trans = transforms.Compose([transforms.ToTensor(),]) data = trans(img) data = data.resize_((1,data.shape[0],data.shape[1],data.shape[2])) # Pass data to GPU if self.use_cuda: data = data.cuda() # Wrap tensors in Variable class, set volatile=True for inference mode and to use minimal memory during inference data = Variable(data, volatile=True) # Forward pass output = self.model(data).data all_boxes = get_region_boxes(output, self.conf_thresh, self.num_classes) for i in range(output.size(0)): # For each image, get all the predictions boxes = all_boxes[i] best_conf_est = -1 # If the prediction has the highest confidence, choose it as our prediction for single object pose estimation for j in range(len(boxes)): if (boxes[j][18] > best_conf_est): box_pr = boxes[j] best_conf_est = boxes[j][18] # Denormalize the corner predictions corners2D_pr = np.array(np.reshape(box_pr[:18], [9, 2]), dtype='float32') corners2D_pr[:, 0] = corners2D_pr[:, 0] * 640 corners2D_pr[:, 1] = corners2D_pr[:, 1] * 480 R_pr, t_pr = pnp(np.array(np.transpose(np.concatenate((np.zeros((3, 1)), self.corners3D[:3, :]), axis=1)), dtype='float32'), corners2D_pr, np.array(self.internal_calibration, dtype='float32')) proj_2d_gt = corners2D_pr R_tmp = [] for i in range(3): for j in range(3): R_tmp.append(R_pr[i][j]) rotation = PyKDL.Rotation(R_tmp[0],R_tmp[1],R_tmp[2],R_tmp[3],R_tmp[4],R_tmp[5],R_tmp[6],R_tmp[7],R_tmp[8]) self.rotation_rpy = rotation.GetRPY() print(self.rotation_rpy) x1 = int(proj_2d_gt[1][0]) y1 = int(proj_2d_gt[1][1]) x2 = int(proj_2d_gt[3][0]) y2 = int(proj_2d_gt[3][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[5][0]) y1 = int(proj_2d_gt[5][1]) x2 = int(proj_2d_gt[7][0]) y2 = int(proj_2d_gt[7][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[1][0]) y1 = int(proj_2d_gt[1][1]) x2 = int(proj_2d_gt[5][0]) y2 = int(proj_2d_gt[5][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[3][0]) y1 = int(proj_2d_gt[3][1]) x2 = int(proj_2d_gt[7][0]) y2 = int(proj_2d_gt[7][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[1][0]) y1 = int(proj_2d_gt[1][1]) x2 = int(proj_2d_gt[2][0]) y2 = int(proj_2d_gt[2][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[3][0]) y1 = int(proj_2d_gt[3][1]) x2 = int(proj_2d_gt[4][0]) y2 = int(proj_2d_gt[4][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[5][0]) y1 = int(proj_2d_gt[5][1]) x2 = int(proj_2d_gt[6][0]) y2 = int(proj_2d_gt[6][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[7][0]) y1 = int(proj_2d_gt[7][1]) x2 = int(proj_2d_gt[8][0]) y2 = int(proj_2d_gt[8][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[2][0]) y1 = int(proj_2d_gt[2][1]) x2 = int(proj_2d_gt[4][0]) y2 = int(proj_2d_gt[4][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[6][0]) y1 = int(proj_2d_gt[6][1]) x2 = int(proj_2d_gt[8][0]) y2 = int(proj_2d_gt[8][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[2][0]) y1 = int(proj_2d_gt[2][1]) x2 = int(proj_2d_gt[6][0]) y2 = int(proj_2d_gt[6][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[4][0]) y1 = int(proj_2d_gt[4][1]) x2 = int(proj_2d_gt[8][0]) y2 = int(proj_2d_gt[8][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2) x1 = int(proj_2d_gt[2][0]) y1 = int(proj_2d_gt[2][1]) x2 = int(proj_2d_gt[8][0]) y2 = int(proj_2d_gt[8][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2) x1 = int(proj_2d_gt[4][0]) y1 = int(proj_2d_gt[4][1]) x2 = int(proj_2d_gt[6][0]) y2 = int(proj_2d_gt[6][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2) x1 = int(proj_2d_gt[3][0]) y1 = int(proj_2d_gt[3][1]) x2 = int(proj_2d_gt[8][0]) y2 = int(proj_2d_gt[8][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2) x1 = int(proj_2d_gt[4][0]) y1 = int(proj_2d_gt[4][1]) x2 = int(proj_2d_gt[7][0]) y2 = int(proj_2d_gt[7][1]) cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2) return ori_img, R_pr, t_pr
output = np.array([x, y, z]) return output sampling_period = 1 / 30 time_for_stretch = 2 target_displacement = 0.02 delta_displacement = -(target_displacement / time_for_stretch) * 1 / 30 translation = PyKDL.Vector(delta_displacement, 0.0, 0.0) translation2 = PyKDL.Vector(-delta_displacement, 0.0, 0.0) """define the waypoint positions for the PSMs for this load test""" cart1 = PyKDL.Vector(0.1399, -0.0674, -0.0864) rot1 = PyKDL.Rotation() rot1 = rot1.Quaternion(0.732, 0.013, 0.677, 0.0710) pos1 = PyKDL.Frame(rot1, cart1) cart2 = PyKDL.Vector(0.14425, -0.0676, -0.08934) rot2 = PyKDL.Rotation() rot2 = rot1.Quaternion(0.725, -0.0026, 0.6881, -0.0166) pos2 = PyKDL.Frame(rot2, cart2) cart3 = PyKDL.Vector(0.14025, -0.0676, -0.08934) rot3 = PyKDL.Rotation() rot3 = rot1.Quaternion(0.14425, -0.0676, -0.08934) pos3 = PyKDL.Frame(rot3, cart3) p2 = dvrk.psm('PSM2')
# move all joints - relative, incremental in joint space p.dmove_joint(np.array([0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0]), interpolate=True) # move in cartesian space # there are only 2 methods available, dmove and move # both accept PyKDL Frame, Vector or Rotation # Absolute translation in cartesian space. p.move( PyKDL.Vector(0.0, 0.0, -0.05), interpolate=True) # move such that end effector becomes at (0, 0, -0.05) # Incremental motion in cartesian space. p.dmove(PyKDL.Vector(0.0, 0.05, 0.0), interpolate=True) # move by 5 cm in Y direction # save current orientation old_orientation = p.get_desired_position().M # save current XYZ position old_position = p.get_desired_position().p # rotate about an axis r = PyKDL.Rotation() r.DoRotX(math.pi * 0.25) # rotate about the X axis p.dmove(r) # p.move(old_orientation) raw_input('Press enter to shutdown') # Stop providing power to the arm p.shutdown()
return def get_pose(frame): x = frame.p.x() y = frame.p.y() z = frame.p.z() q1,q2,q3,q4 = frame.M.GetQuaternion() output = (x,y,z,q1,q2,q3,q4) return output """ MTM home rough position """ ''' We use this to initialize a position for the MTMR''' MTMR_cart = PyKDL.Vector(0.055288515671, -0.0508310176185, -0.0659661913251) MTMR_rot = PyKDL.Rotation() MTMR_rot = MTMR_rot.Quaternion(0.750403138242, -0.0111643539824, 0.657383142871, -0.0679550644629) MTMR_pos = PyKDL.Frame(MTMR_rot, MTMR_cart) c = dvrk.console() p2 = dvrk.psm('PSM2') mtm = dvrk.mtm('MTMR') print('initializing approximate MTMR position') mtm.move(MTMR_pos) c.teleop_start() sub = rospy.Subscriber('/dvrk/footpedals/camera', Joy, trigger_callback) trigger = False trigger_time = 0 filename = './manipulator_homing/'
def fromMatrix(m): return PyKDL.Frame(PyKDL.Rotation(m[0,0], m[0,1], m[0,2], m[1,0], m[1,1], m[1,2], m[2,0], m[2,1], m[2,2]), PyKDL.Vector(m[0,3], m[1, 3], m[2, 3]))