def localCb(self, data): self.localPose.setPoseStamped(data) if(not (self.enuTickPose.isNone() or self.offset is None)): t = self.localPose.getTranslation() q = self.enuTickPose.getQuaternion() q = quaternion_matrix(q) t = translation_matrix(t) self.localEnuPose.setMatrix(numpy.dot(q,t)) t = self.localEnuPose.getTranslation() t[0] -= self.offset[0] t[1] -= self.offset[1] t[2] -= self.offset[2] q = self.localEnuPose.getQuaternion() self.localEnuPose.setTranslationQuaternion(t, q) p = PointStamped() p.point.x = self.offset[0] p.point.y = self.offset[1] p.point.z = self.offset[2] p.header = Header(0, rospy.rostime.get_rostime(), "world") self.offsetPub.publish(p) self.localEnuPub.publish(self.localEnuPose.getPoseStamped())
def get_base_origin(self): """ :return: The centre of the arm's base :type: PointStamped """ current_pose = self.__base_group.get_current_joint_values() result = PointStamped() result.header.frame_id = "/odom_combined" result.point = Point(current_pose[0], current_pose[1], 0) return result
def get_fingertip(hand_pose): """ Calculates the point between the finger tips, where objects will be hold. :param hand_pose: hand pose :type: PoseStamped :return: point between fingers :type: PointStamped """ grasp_point = PointStamped() grasp_point.point = set_vector_length(hand_length + finger_length, Point(1, 0, 0)) grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point) grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point) grasp_point.header.frame_id = hand_pose.header.frame_id return grasp_point
def get_center_of_mass(self, collision_object): """ Calculates the centre of a collision object. :param collision_object: CollisionObject :type: CollisionObject :return: centre of mass :type: PointStamped """ p = PointStamped() p.header.frame_id = "/odom_combined" for pose in collision_object.primitive_poses: p.point = add_point(p.point, pose.position) p.point = multiply_point(1.0 / len(collision_object.primitive_poses), p.point) return p
def get_place_position_handle(collision_object, destination, transform_func, d): """ Calculates place positions for a composition of collision objects. :param collision_object: object to be placed :type: CollisionObject :param destination: :type: PointStamped :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to) :param grasp: PoseStamped :type: PoseStamped :param d: dist from the grasped point to the tcp frame :type: float :return: list of possible place positions :type: [PoseStamped] """ angle = 0 place_pose = destination.point p2 = PointStamped() p2.header.frame_id = collision_object.id p2 = transform_func(p2, "tcp") place_pose.z = collision_object.primitives[0].dimensions[ shape_msgs.msg.SolidPrimitive.CYLINDER_HEIGHT] / 2 + \ collision_object.primitives[1].dimensions[ shape_msgs.msg.SolidPrimitive.BOX_X] + safe_place place_pose.z += abs(p2.point.y) place_poses = make_scan_pose(place_pose, d, angle) #rotate place poses, depending on where the centroid of the object is, relative to the gripper if p2.point.y < 0: for i in range(0, len(place_poses)): place_poses[i].pose.orientation = rotate_quaternion( place_poses[i].pose.orientation, pi, 0, 0) return place_poses
def get_place_position_for_single_object(collision_object, destination, transform_func, grasp, d): """ Calculates place positions for a non composition of collision objects. :param collision_object: object to be placed :type: CollisionObject :param destination: :type: PointStamped :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to) :param grasp: PoseStamped :type: PoseStamped :param d: dist from the grasped point to the tcp frame :type: float :return: list of possible place positions :type: [PoseStamped] """ angle = get_pitch(grasp.pose.orientation) z_o = abs(grasp.pose.position.z) - (sin(angle) * d) place_pose = destination.point place_pose.z = z_o + safe_place diff = abs(pi / 2 - angle) if 0 <= diff <= 0.1: place_poses = make_scan_pose(place_pose, d, angle, n=2) else: place_poses = make_scan_pose(place_pose, d, angle) p2 = PointStamped() p2.header.frame_id = collision_object.id p2 = transform_func(p2, "tcp") rospy.logwarn(p2) if p2.point.y < 0: for i in range(0, len(place_poses)): place_poses[i].pose.orientation = rotate_quaternion( place_poses[i].pose.orientation, pi, 0, 0) return place_poses
def test_task3(mani): dest = PointStamped() dest.header.frame_id = "/odom_combined" dest.point = Point(0.85, -0.85, 0) mani.place_and_move(dest)