def get_obstacle_pose( cap_front, cap_rear, obs_front, obs_rear, obs_gps_to_centroid, velodyne_to_front, cap_yaw_error_rad=0, cap_pitch_error_rad=0): # calculate capture yaw in ENU frame and setup correction rotation cap_front_v = dict_to_vect(cap_front) cap_rear_v = dict_to_vect(cap_rear) cap_yaw = get_yaw(cap_front_v, cap_rear_v) cap_yaw += cap_yaw_error_rad rot_cap = kd.Rotation.EulerZYX(-cap_yaw, -cap_pitch_error_rad, 0) obs_rear_v = dict_to_vect(obs_rear) if obs_front: obs_front_v = dict_to_vect(obs_front) obs_yaw = get_yaw(obs_front_v, obs_rear_v) # use the front gps as the obstacle reference point if it exists as it's closers # to the centroid and mounting metadata seems more reliable cap_to_obs = obs_front_v - cap_front_v else: cap_to_obs = obs_rear_v - cap_front_v # transform capture car to obstacle vector into capture car velodyne lidar frame res = rot_cap * cap_to_obs res += list_to_vect(velodyne_to_front) # obs_gps_to_centroid is offset for front gps if it exists, otherwise rear obs_gps_to_centroid_v = list_to_vect(obs_gps_to_centroid) if obs_front: # if we have both front + rear RTK calculate an obstacle yaw and use it for centroid offset obs_rot_z = kd.Rotation.RotZ(obs_yaw - cap_yaw) centroid_offset = obs_rot_z * obs_gps_to_centroid_v else: # if no obstacle yaw calculation possible, treat rear RTK as centroid and offset in Z only obs_rot_z = kd.Rotation() centroid_offset = kd.Vector(0, 0, obs_gps_to_centroid_v[2]) res += centroid_offset return frame_to_dict(kd.Frame(obs_rot_z, res))
def read_data(bag, arm): ''' Returns tuple (time, theta, theta_ref, angular_vel, angular_vel_ref, angular_acc, angular_acc_ref, s, gripper_pos_command, gripper_vel_command, gripper_pos) ''' global alpha time = np.array([]) frame = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, 1.22), PyKDL.Vector(0.0, 0.0, 0.0)) force_x = np.array([]) force_x_t = np.array([]) force_y = np.array([]) force_y_t = np.array([]) force_z = np.array([]) torque_x = np.array([]) torque_x_t = np.array([]) torque_y = np.array([]) torque_y_t = np.array([]) torque_z = np.array([]) for topic, msg, t in bag.read_messages( topics=['/ft/' + arm + '_gripper_motor']): time = np.append(time, msg.header.stamp.to_sec()) force_x = np.append(force_x, msg.wrench.force.x) force_y = np.append(force_y, msg.wrench.force.y) #aligned_Fx = msg.wrench.force.x*math.cos(alpha) - msg.wrench.force.y*math.sin(alpha) #aligned_Fy = msg.wrench.force.x*math.sin(alpha) + msg.wrench.force.y*math.cos(alpha) #force_x_t = np.append(force_x_t, aligned_Fx) #force_y_t = np.append(force_y_t, aligned_Fy) #force_z = np.append(force_z, msg.wrench.force.z) ## torque_x = np.append(torque_x, msg.wrench.torque.x) ## torque_y = np.append(torque_y, msg.wrench.torque.y) ## aligned_Tx = msg.wrench.torque.y*math.sin(alpha) + msg.wrench.torque.x*math.cos(alpha) ## aligned_Ty = msg.wrench.torque.y*math.cos(alpha) - msg.wrench.torque.x*math.sin(alpha) ## torque_x_t = np.append(torque_x_t, aligned_Tx) ## torque_y_t = np.append(torque_y_t, aligned_Ty) #torque_z = np.append(torque_z, msg.wrench.torque.z) #return time, force_x, force_y, force_z, torque_x, torque_y, torque_z return time, force_x, force_y, #force_x_t, force_y_t, torque_x, torque_y, torque_x_t, torque_y_t
def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed): ''' this function assumes everything is represented in the quaternions in the /base_link frame ''' if object_name.endswith('_scan'): object_name = object_name[:-5] dictObj = objDict() objSpec = dictObj.getEntry(object_name) F_bin_frame = posemath.fromTf(bin_frame) F_obj_frame = posemath.fromTf(obj_frame) objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0) objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0) objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0) binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0) binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0) binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0) rRProj = kdl.dot(objRed, binRed) gRProj = kdl.dot(objGreen, binRed) bRProj = kdl.dot(objBlue, binRed) tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)] if simtrackUsed: for i in range(3): if i in objSpec.invalidApproachAxis: tmpApproach1[i] = 0 tmpApproach2 = [rRProj, gRProj, bRProj] axisApproach = tmpApproach1.index(max(tmpApproach1)) objAxes = [objRed, objGreen, objBlue] tmpGrasp1 = [] for i in range(3): if simtrackUsed: if i == axisApproach or i in objSpec.invalidGraspAxis: tmpGrasp1.append(0) continue tmpGrasp1.append(kdl.dot(objAxes[i], binBlue)) tmpGrasp2 = [abs(t) for t in tmpGrasp1] axisGrasp = tmpGrasp2.index(max(tmpGrasp2)) return axisApproach, tmpApproach2[axisApproach] / abs( tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp] / abs( tmpGrasp1[axisGrasp])
def __init__( self, objs, bases, pos, rot, pos_tol, ): self.objs = objs self.bases = bases self.pos_tol = pos_tol self.pos = pos self.rot = rot # Compute the relative transformation from the "base" where we hope to # find the object (or, you know, where we're afraid to find it...) pg = kdl.Vector(*self.pos) Rg = kdl.Rotation.Quaternion(*self.rot) self.T = kdl.Frame(Rg, pg)
def getActualPosition(self): reply = yarp.Bottle() reply.clear() request = yarp.Bottle() request.clear() request.addString('get actual_position') self.rpc.write(request, reply) position = kdl.Vector() if reply.size() != 3: raise Exception('Unexpected reply size') for i in xrange(3): position[i] = reply.get(i).asDouble() return position
def fromStr(self, line): tab = line.split() self.successful = bool(tab[0]) contacts_count = int(tab[1]) self.contacts = [] for idx in range(0, contacts_count): f_idx = int(tab[2 + idx * 8 + 0]) px = float(tab[2 + idx * 8 + 1]) py = float(tab[2 + idx * 8 + 2]) pz = float(tab[2 + idx * 8 + 3]) qx = float(tab[2 + idx * 8 + 4]) qy = float(tab[2 + idx * 8 + 5]) qz = float(tab[2 + idx * 8 + 6]) qw = float(tab[2 + idx * 8 + 7]) self.contacts.append([ f_idx, PyKDL.Frame(PyKDL.Rotation.Quaternion(qx, qy, qz, qw), PyKDL.Vector(px, py, pz)) ])
def skeleton_handler(self, msg): for joint in msg.name: #self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)] self.skeleton['position'][joint] = KDL.Vector( msg.position[msg.name.index(joint)].x, msg.position[msg.name.index(joint)].y, msg.position[msg.name.index(joint)].z) if joint == 'right_hand': if self.skeleton['position'][joint][0] < -0.15: keyboard.press(Key.left) time.sleep(abs(self.skeleton['position'][joint][0])) keyboard.release(Key.left) elif self.skeleton['position'][joint][0] > 0.15: keyboard.press(Key.right) time.sleep(abs(self.skeleton['position'][joint][0])) keyboard.release(Key.right) else: keyboard.release(Key.right) keyboard.release(Key.left)
def look_at_segmentation_area(robot, entity, volume=None): """ Has a robot look at a certain object and possibly a volume :param robot: robot object :param entity: entity to look at :param volume: string indicating the specific volume to look at (e.g., 'on_top_on' or 'shelf3') """ # Determine the height of the head target # Start with a default # Check if we have areas: use these if volume in entity.volumes: search_volume = entity.volumes[volume] x_obj = 0.5 * (search_volume.min_corner.x() + search_volume.max_corner.x()) y_obj = 0.5 * (search_volume.min_corner.y() + search_volume.max_corner.y()) z_obj = search_volume.min_corner.z() lookat_pos_map = entity.pose.frame * kdl.Vector(x_obj, y_obj, z_obj) x = lookat_pos_map.x() y = lookat_pos_map.y() z = lookat_pos_map.z() else: # Look at the top of the entity to inspect pos = entity.pose.frame.p x = pos.x() y = pos.y() z = pos.z() + entity.shape.z_max # Point the head at the right direction robot.head.look_at_point(VectorStamped(x, y, z, "/map"), timeout=0) # Make sure the spindle is at the appropriate height if we are AMIGO if robot.robot_name == "amigo": # Send spindle goal to bring head to a suitable location # Correction for standard height: with a table heigt of 0.76 a spindle position # of 0.35 is desired, hence offset = 0.76-0.35 = 0.41 # Minimum: 0.15 (to avoid crushing the arms), maximum 0.4 spindle_target = max(0.15, min(z - 0.41, robot.torso.upper_limit[0])) robot.torso._send_goal([spindle_target], timeout=0) robot.torso.wait_for_motion_done() robot.head.wait_for_motion_done()
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): """ Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. @return True on success """ transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), PyKDL.Vector(-x, -y, -z)) tip_pose = self.get_tip_pose() tip_pose_kdl = posemath.fromMsg(tip_pose) final_pose = toMsg(tip_pose_kdl * transform) self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([final_pose]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True
def __init__(self, pos, rot, gripper, goal=None, cartesian_vel=0.35, angular_vel=0.5): self.pos = pos self.rot = [r * np.pi for r in rot] self.goal = goal self.gripper = gripper self.cartesian_vel = cartesian_vel self.angular_vel = angular_vel print(self.pos, self.rot) pg = kdl.Vector(*self.pos) Rg = kdl.Rotation.RPY(*self.rot) self.T = kdl.Frame(Rg, pg)
def _loadFromFile(self): self.data = np.genfromtxt(self.getGTName(), delimiter=self.delimiter, dtype=None, names=True) self.objs = {} for row in self.data: name = row[0] if 'fake' in name: continue size = [row[1], row[2], row[3]] p = PyKDL.Vector(row[4], row[5], row[6]) rot = PyKDL.Rotation.Quaternion(row[7], row[8], row[9], row[10]) color = np.array([row[11], row[12], row[13]]) self.objs[name] = { "vobj": VirtualObject(frame=PyKDL.Frame(rot, p), size=size), "color": color }
def orientation_feature(feature): eps = 1e-10 # some small number for alignment test direction = msg2vec(feature.direction) scale = direction.Norm() axis = kdl.Vector(0,0,1) * direction angle = direction.z() laxis = axis.Norm() if laxis > eps and scale > eps: l = sqrt((1 - angle / scale) / 2) / laxis qu = sqrt((1 + angle / scale) / 2) q = [axis.x()*l, axis.y()*l, axis.z()*l, qu] else: # aligned with x/z-axis or zero-length: no rotation needed q = [0, 0, 0, 1] return q
def computeTSRsForSphere(radius, angleStep=30*numpy.pi/180.0): radial_vector = kdl.Vector(0.0, 0.0, 1.0) * radius x_angle_grid = numpy.linspace(0.0, 2*numpy.pi, int(2*numpy.pi/angleStep)) y_angle_grid = numpy.copy(x_angle_grid) tsrs = [] for x_angle in x_angle_grid: for y_angle in y_angle_grid: R = kdl.Rotation.RPY(x_angle, y_angle, 0.0) pos = R * radial_vector rot = R * kdl.Rotation.RotX(numpy.pi) tsrs.append(kdlFrameToBoxTSR(kdl.Frame(rot, pos))) return tsrs
def pose_msg_to_kdl_frame(pose): """ Convert a geometry_msgs.msg.Pose message to a PyKDL.Frame object :param pose: Pose to be converted :type pose: geometry_msgs.msg.Pose :rtype: PyKDL.Frame >>> pose = gm.Pose(gm.Point(1, 2, 3), gm.Quaternion(1, 0, 0, 0)) >>> frame = pose_msg_to_kdl_frame(pose) >>> frame.p [ 1, 2, 3] >>> frame.M.GetQuaternion() (1.0, 0.0, 0.0, 0.0) """ rot = kdl.Rotation.Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) trans = kdl.Vector(pose.position.x, pose.position.y, pose.position.z) return kdl.Frame(rot, trans)
def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1.0 t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0) t.header.stamp = builtin_interfaces.msg.Time(sec=2) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2), rclpy.time.Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1, 2, 3) out = b.transform( tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) out = b.transform( tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4)
def generate(self): for ori_idx in range(len(orientations)): T_H_Hd = orientations[ori_idx] T_H_Od = T_H_Hd * self.T_H_O for surf_pt_idx in range(len(self.surface_points_obj)): surf_pt = self.surface_points_obj[surf_pt_idx] if not surf_pt.allowed: continue pt = T_H_Od * surf_pt.pos vol_idx = self.getVolIndex(pt) if vol_idx != None: if not ori_idx in self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]]: self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]][ori_idx] = [surf_pt.id] else: self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]][ori_idx].append(surf_pt.id) print "transforming the volumetric map..." for xi in range(self.vol_samples_count): print xi for yi in range(self.vol_samples_count): for zi in range(self.vol_samples_count): for ori in self.vol_samples[xi][yi][zi]: planes = 0 edges = 0 points = 0 norm = PyKDL.Vector() for pt_id in self.vol_samples[xi][yi][zi][ori]: norm += self.surface_points_obj[pt_id].normal if self.surface_points_obj[pt_id].is_plane: planes += 1 if self.surface_points_obj[pt_id].is_edge: edges += 1 if self.surface_points_obj[pt_id].is_point: points += 1 norm.Normalize() if planes >= edges and planes >= points: self.vol_samples[xi][yi][zi][ori] = (norm, 0) elif edges >= planes and edges >= points: self.vol_samples[xi][yi][zi][ori] = (norm, 1) else: self.vol_samples[xi][yi][zi][ori] = (norm, 2)
def intersect(self, ray_dir_frame, **kwargs): if 'normal' in kwargs and kwargs['normal']: normal = kwargs['normal'] else: normal = self._normal if 'point' in kwargs and kwargs['point']: point = kwargs['point'] else: point = self._point if not (normal and point): return None u = ray_dir_frame.p - ray_dir_frame * kdl.Vector(1.0, 0.0, 0.0) # normal.Normalize() n = normal w = ray_dir_frame.p - point # - n * self._distance D = kdl.dot(n, u) N = -kdl.dot(n, w) # Check if the vector is parallel to the plane if math.fabs(D) > sys.float_info.epsilon: sI = N / D # Is the intersection on the back? if sI >= 0.0: return None # Point on the plane p = ray_dir_frame.p + sI * u yaw = math.atan2(p.y(), p.x()) # Turn along the pointed direction in horizontal plane q = kdl.Rotation.RPY(0.0, 0.0, yaw) return kdl.Frame(q, p) return None
def inverseKinematics(robotChain, pos, rot, qGuess=None, minJoints=None, maxJoints=None): """ Perform inverse kinematics Args: robotChain: robot's chain object pos: 1 x 3 or 3 x 1 array of the end effector position. rot: 3 x 3 array of the end effector rotation qGuess: guess values for the joint positions minJoints: minimum value of the position for each joint maxJoints: maximum value of the position for each joint Returns: list of joint positions or None (no solution) """ # print("inside inverse: ", pos, " ; ", rot) posKdl = kdl.Vector(pos[0], pos[1], pos[2]) rotKdl = 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]) frameKdl = kdl.Frame(rotKdl, posKdl) numJoints = robotChain.getNrOfJoints() minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints minsKdl = jointListToKdl(minJoints) maxsKdl = jointListToKdl(maxJoints) fkKdl = kdl.ChainFkSolverPos_recursive(robotChain) ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain) ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl, fkKdl, ikVKdl) if qGuess is None: # use the midpoint of the joint limits as the guess lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.) upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.) qGuess = (lowerLim + upperLim) / 2.0 qGuess = np.where(np.isnan(qGuess), [0.]*len(qGuess), qGuess) qKdl = kdl.JntArray(numJoints) qGuessKdl = jointListToKdl(qGuess) if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0: return jointKdlToList(qKdl) else: return None
def zero_forces(PSM, epsilon): home = False Kp = 0.005 Kd = 0.003 F_old = force_feedback F_array = npm.repmat(force_feedback, 10, 1) while home == False: Fx = force_feedback[0] Fy = force_feedback[1] Fz = force_feedback[2] Fx_d = Fx - F_old[0] Fy_d = Fy - F_old[1] Fz_d = Fz - F_old[2] F_old = [Fx, Fy, Fz] F_array[0, :] = force_feedback F_array[1, :] = F_old F_array[2:-1, :] = F_array[1:-2, :] #F_average = (np.array(force_feedback) + np.array(F_old)+ np.array(F1)+np.array(F2)+np.array(F3)+np.array(F4))/6 F_median = np.median(F_array, 0) F_average = np.mean(F_array, 0) if np.linalg.norm(F_median) > epsilon: #if np.abs(Fx) > 0.05: p2.dmove( PyKDL.Vector(Kp * Fx + Kd * Fx_d, Kp * Fy + Kd * Fy_d, Kp * Fz + Kd * Fz_d)) #p2.dmove(PyKDL.Vector(Kp*Fx+Kd*Fx_d, 0, 0)) #print(np.linalg.norm(F_median)) #print(np.linalg.norm(force_feedback)) #print(force_feedback) #print(str(Kp*Fx+Kd*Fx_d) + ',' +str(Kp*Fy+Kd*Fy_d) + ',' +str(-(Kp*Fz+Kd*Fz_d))) else: print(F_median) print(F_average) home = True
def create_right_chain(self): ch = kdl.Chain() self.right_arm_base_offset_from_torso_lift_link = np.matrix([0., -0.188, 0.]).T # shoulder pan ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.1,0.,0.)))) # shoulder lift ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.)))) # upper arm roll ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.4,0.,0.)))) # elbox flex ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.0,0.,0.)))) # forearm roll ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.321,0.,0.)))) # wrist flex ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.)))) # wrist roll ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,0.)))) return ch
def is_at_landing_spot(self): rp = kdl.Vector(self.robot_current_pose.pose.position.x, self.robot_current_pose.pose.position.y, 0.0) ls = self.landing_spot d = (rp - ls).Norm() if math.isnan(d): rospy.loginfo('Landing anywhere') return True # rospy.loginfo('Robot pose: {}'.format(rp)) if d < (self.landing_tolerance * 1.5): rospy.loginfo('Distance to landing spot: {}'.format(d)) if d < self.landing_tolerance: return True else: return False
def _get_goal(self): if self._command is None or rospy.get_time() - self._last_reference_update > 0.1: return self._last_goal # Compute the step from the input velocity step_frame = PyKDL.Frame( PyKDL.Rotation.RPY(self._command[3], self._command[4], self._command[5]), PyKDL.Vector(self._command[0], self._command[1], self._command[2])) next_goal = self._last_goal * step_frame g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()] g_quat = next_goal.M.GetQuaternion() if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None: return next_goal else: return self._last_goal
def getFrameFromPose(pose): """ Return a PyKDL.Frame object from a Pose(). @type pose: geometry_msgs/Pose @param pose: Pose. @rtype: PyKDL.Frame """ V = kdl.Vector(pose.position.x, pose.position.y, pose.position.z) x = pose.orientation.x y = pose.orientation.y z = pose.orientation.z w = pose.orientation.w qmag = math.sqrt(x**2 + y**2 + z**2 + w**2) R = kdl.Rotation.Quaternion(pose.orientation.x / qmag, pose.orientation.y / qmag, pose.orientation.z / qmag, pose.orientation.w / qmag) return kdl.Frame(R, V)
def pubVirtualBowlcenPose(self): f = PyKDL.Frame.Identity() f.p = PyKDL.Vector(0.5, 0.2, -0.2) f.M = PyKDL.Rotation.Quaternion(0, 0, 0, 1) # frame pub -------------------------------------- ps = PoseStamped() ps.header.frame_id = 'torso_lift_link' ps.header.stamp = rospy.Time.now() ps.pose.position.x = f.p[0] ps.pose.position.y = f.p[1] ps.pose.position.z = f.p[2] ps.pose.orientation.x = f.M.GetQuaternion()[0] ps.pose.orientation.y = f.M.GetQuaternion()[1] ps.pose.orientation.z = f.M.GetQuaternion()[2] ps.pose.orientation.w = f.M.GetQuaternion()[3] self.bowl_cen_pose_pub.publish(ps)
def moveToHandle(): print "Ruch do klamki..." DoorR = velma.getTf("B", "right_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorR_x, doorR_y, doorR_z = DoorR.p DoorL = velma.getTf("B", "left_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorL_x, doorL_y, doorL_z = DoorL.p position = velma.getLastJointState() Goal = PyKDL.Vector(doorL_x,doorL_y,doorR_z+0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal) if not velma.moveCartImpRight([T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam klamki :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def __init__(self, robot_name="", params_dict={}): self.robot_name = robot_name self.is_active = False self.done = False # Controller Parameters if params_dict == {} or params_dict == None: params_dict = self.DEFAULT_PARAMETERS self.params = params_dict self.damp_force_threshold = self._param("damp_force_threshold", params_dict) self.damp_magnitude = self._param("damp_magnitude", params_dict) self.initial_velocity = self._param("velocity", params_dict) self.velocity = 0 self.axis = PyKDL.Vector() self.last_force_msg = Twist() self.force_limit = [2, 2, 2]
def move_table(userdata=None): """ 'Locks' a locking designator """ # For now, don't do anything return "done" # Move away the cabinet robot.ed.update_entity(id="cabinet", frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(12.0, 0, 0)), frame_id="map")) # Determine where to perform the challenge robot_pose = robot.base.get_location() ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm()) # Update the world model robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1]) return "done"
def get_pointer(self, ws_shape, ray_kdl_frame, pass_point=None, cache=False): robot_kdl_frame = self.frame_from_tf(self.human_frame, self.robot_frame) if not pass_point: if not ws_shape.point: if robot_kdl_frame: # Initialize with robot's current position ws_shape.point = copy.deepcopy(robot_kdl_frame.p) else: return None pass_point = ws_shape.point ray_origin_kdl_frame = self.frame_from_tf(self.human_frame, self.ray_origin_frame) shape_origin_kdl_frame = self.frame_from_tf(self.human_frame, ws_shape.frame_id) pointer_pose = None if pass_point and ray_origin_kdl_frame and shape_origin_kdl_frame: actual_pass_point = pass_point - copy.deepcopy(shape_origin_kdl_frame.p) if cache: # or not ws_shape.point: ws_shape.point = pass_point if isinstance(ws_shape, Plane): normal = None if ws_shape is self.vis_plane: tmp_point = copy.deepcopy(ray_origin_kdl_frame.p) robot_dir = actual_pass_point - tmp_point normal = kdl.Vector(robot_dir.x(), robot_dir.y(), 0.0) if cache: ws_shape.normal = normal pointer_pose = ws_shape.ray_to_location(ray_kdl_frame, normal=normal, point=actual_pass_point) else: pointer_pose = ws_shape.ray_to_location(ray_kdl_frame, point=actual_pass_point) return pointer_pose
def transform_frame_to_map(self, cloud, tf): rospy.loginfo("creating transformer") if (tf is None): self.listener = tf.TransformListener() else: self.listener = TransformationStore().msg_to_transformer(tf) rospy.sleep(5) self.child_camera_frame = cloud.header.frame_id rospy.loginfo("doing conversion") t = self.listener.getLatestCommonTime("map", self.child_camera_frame) tr_r = self.listener.lookupTransform("map", self.child_camera_frame, t) tr = Transform() tr.translation = Vector3(tr_r[0][0], tr_r[0][1], tr_r[0][2]) tr.rotation = Quaternion(tr_r[1][0], tr_r[1][1], tr_r[1][2], tr_r[1][3]) #self.transform_frame_to_map = tr tr_s = TransformStamped() tr_s.header = std_msgs.msg.Header() tr_s.header.stamp = rospy.Time.now() tr_s.header.frame_id = 'map' tr_s.child_frame_id = self.child_camera_frame tr_s.transform = tr t_kdl = self.transform_to_kdl(tr_s) points_out = [] for p_in in pc2.read_points(cloud, field_names=["x", "y", "z", "rgb"]): p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2]) points_out.append([p_out[0], p_out[1], p_out[2], p_in[3]]) fil_fields = [] for x in cloud.fields: if (x.name in "x" or x.name in "y" or x.name in "z" or x.name in "rgb"): fil_fields.append(x) res = pc2.create_cloud(cloud.header, fil_fields, points_out) return res
def __init__(self): # Get the node params self.linear_multiplier = rospy.get_param('~linear_multiplier') self.angular_multiplier = rospy.get_param('~angular_multiplier') self.broadcast_rate = rospy.get_param('~broadcast_rate') self.body_fixed = rospy.get_param('~body_fixed') # Initialize the frame we're going to publish xyzw = [float(f) for f in rospy.get_param('~xyzw', '0 0 0 1').split()] xyzw_offset = [ float(f) for f in rospy.get_param('~xyzw_offset', '0 0 0 1').split() ] xyz = [float(f) for f in rospy.get_param('~xyz', '0 0 0').split()] rospy.logwarn("xyzw: " + str(xyzw)) rospy.logwarn("xyz: " + str(xyz)) self.transform = PyKDL.Frame( PyKDL.Rotation.Quaternion(*xyzw) * PyKDL.Rotation.Quaternion(*xyzw_offset), PyKDL.Vector(*xyz)) self.transform_out = PyKDL.Frame() self.rotation = self.transform.M.GetQuaternion() self.time = rospy.Time.now() self.frame_id = rospy.get_param('~frame_id') self.child_frame_id = rospy.get_param('~child_frame_id') # Synchronization self.broadcast_lock = threading.Lock() # Initialze TF structures self.broadcaster = tf.TransformBroadcaster() # Subscribe to twist inputs self.twist_sub = rospy.Subscriber("twist", geometry_msgs.Twist, self.twist_cb) self.twiststamped_sub = rospy.Subscriber("twist_stamped", geometry_msgs.TwistStamped, self.twiststamped_cb) # Broadcast the frame at a given rate self.broadcast_thread = threading.Thread(target=self.broadcast_loop) self.broadcast_thread.start()