sphere_poses.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on 5/22/15

@author: sampfeiffer

sphere.py contains...
"""
__author__ = 'sampfeiffer'


import rospy
from tf.transformations import euler_from_quaternion, quaternion_from_euler, quaternion_about_axis, unit_vector
from moveit_msgs.msg import Grasp, GripperTranslation
from trajectory_msgs.msg import JointTrajectory
from geometry_msgs.msg import PoseStamped, Vector3Stamped, Pose, Quaternion, PoseArray, Vector3, Point
from std_msgs.msg import ColorRGBA
import math
from visualization_msgs.msg import MarkerArray, Marker
import numpy as np



# http://stackoverflow.com/questions/17044296/quaternion-rotation-without-euler-angles
def quaternion_from_vectors(v0, v1):
    if type(v0) == Point():
        v0 = [v0.x, v0.y, v0.z]
    if type(v1) == Point():
        v1 = [v1.x, v1.y, v1.z]
#     inline QuaternionT<T> QuaternionT<T>::CreateFromVectors(const Vector3<T>& v0, const Vector3<T>& v1)
# {
#
#     Vector3<T> c = v0.Cross(v1);
    c = np.cross(v0, v1)
#     T d = v0.Dot(v1);
    d = np.dot(v0, v1)
#     T s = std::sqrt((1 + d) * 2);
    s = math.sqrt((1.0 + d) * 2)
    if s == 0.0:
        #print "s == 0.0, we cant compute"
        return [0.0, 0.0, 0.0, 1.0]
#
#     QuaternionT<T> q;
    q = [0.0, 0.0, 0.0, 0.0]
#     q.x = c.x / s;
    q[0] = c[0] / s
#     q.y = c.y / s;
    q[1] = c[1] / s
#     q.z = c.z / s;
    q[2] = c[2] / s
#     q.w = s / 2.0f;
    q[3] = s / 2.0
#     return q;
    return q
# }



if __name__ == "__main__":
    rospy.init_node('test_sphere_poses')

    # Data we will need for fulfilling later:
    object_pose = PoseStamped()
    object_pose.header.frame_id = '/base_link'
    object_pose.pose.position = Point(0.0, 0.0, 0.0)
    object_pose.pose.orientation = Quaternion(0,0,0,1)
    time_for_motions = 3.0 # seconds
    min_motor_pos = 0.0
    max_motor_pos = 1.0
    joint_names = ['finger_1_joint', 'finger_2_joint']
    grasping_frame = 'tool_link'
    min_distances = 0.0
    desired_distances = 1.0
    vector_front = Vector3Stamped()
    vector_front.header.frame_id = grasping_frame
    vector_front.vector.x = 1.0
    vector_front.vector.y = 0.0
    vector_front.vector.z = 0.0
    vector_up = Vector3Stamped()
    vector_up.header.frame_id = grasping_frame
    vector_up.vector.x = 0.0
    vector_up.vector.y = 0.0
    vector_up.vector.z = 1.0



    # A full grasp message that will need to be fulfilled
    g = Grasp()
    g.id = ''
    g.pre_grasp_posture = JointTrajectory() # Position of the joints pre_grasp e.g.: open gripper
    g.grasp_posture = JointTrajectory() # Position of the joints in grasp e.g.: closed gripper
    g.grasp_pose = PoseStamped() # The pose of the gripper, should be based on the pose of the object
    g.grasp_quality = 0.
    g.pre_grasp_approach = GripperTranslation()
    g.pre_grasp_approach.direction = Vector3Stamped() # The direction is in relation to the grasping frame, I think
    g.pre_grasp_approach.direction.header = grasping_frame
    g.pre_grasp_approach.desired_distance = desired_distances
    g.pre_grasp_approach.min_distance = min_distances
    g.post_grasp_retreat = GripperTranslation()
    g.post_grasp_retreat.direction = Vector3Stamped()
    g.post_grasp_retreat.direction.header.frame_id = grasping_frame
    g.post_grasp_retreat.desired_distance = desired_distances
    g.post_grasp_retreat.min_distance = min_distances
    g.post_place_retreat = GripperTranslation()
    g.post_place_retreat.direction = Vector3Stamped()
    g.post_place_retreat.direction.header = grasping_frame
    g.post_place_retreat.desired_distance = desired_distances
    g.post_place_retreat.min_distance = min_distances
    g.max_contact_force = 0.
    g.allowed_touch_objects = []

    # Compute all the points of the sphere with step of 1 deg
    # http://math.stackexchange.com/questions/264686/how-to-find-the-3d-coordinates-on-a-celestial-spheres-surface
    radius = desired_distances
    ori_x = object_pose.pose.position.x
    ori_y = object_pose.pose.position.y
    ori_z = object_pose.pose.position.z
    sphere_poses = []
    print "Creating points"
    for altitude in range(0, 360, 5): # altitude is yaw
        altitude = math.radians(altitude)
        for azimuth in range(0, 360, 5): # azimuth is pitch
            azimuth = math.radians(azimuth)
            # This gets all the positions correctly
            x = ori_x + radius * math.cos(azimuth) * math.cos(altitude)
            y = ori_y + radius * math.sin(altitude)
            z = ori_z + radius * math.sin(azimuth) * math.cos(altitude)
            #current_pose = Pose(Point(0,0,0), Quaternion(*quaternion_from_euler(0.0, -azimuth, altitude))) # This gets all the orientations correctly over 0,0,0
            #dummy_vec = [radius, 0.0, 0.0]
            #q = quat_from_two_vectors(dummy_vec, [x,y,z])
            #current_pose = Pose(Point(x,y,z), Quaternion(*quaternion_from_euler(0.0, -azimuth, altitude))) # fucks up orientations as they are not based on the same initial point
            #q = rviz_quat_from_two_vec([radius, 0.0, 0.0], [x,y,z])
            q = quaternion_from_vectors([radius, 0.0, 0.0], [x,y,z])
            current_pose = Pose(Point(x,y,z), Quaternion(*q))
            sphere_poses.append(current_pose)
    print "Done."


    poses_pub = rospy.Publisher('/sphere_poses', PoseArray, latch=True)
    pa = PoseArray()
    pa.header.frame_id = 'base_link'
    for pose in sphere_poses:
        pa.poses.append(pose)

    poses_pub.publish(pa)

    rospy.spin()