skeleton from openni to ik for pepper with bio_ik

skeleton_to_ik.py

#! /usr/bin/env python

import rospy
from tf.listener import TransformerROS
from tf2_ros import LookupException, TransformListener, TransformBroadcaster, ExtrapolationException
import moveit_msgs.msg
import moveit_msgs.srv
import trajectory_msgs.msg
from geometry_msgs.msg import Point, PointStamped, PoseStamped, Quaternion, Point32, TransformStamped
from sensor_msgs.msg import PointCloud
from visualization_msgs.msg import Marker
import numpy as np
import math
import bio_ik_msgs.msg
import bio_ik_msgs.srv

from quaternion_from_points import quaternion_from_three_points

"""
Skeleton to IK and send.
"""


class SkeletonToIK(object):
    def __init__(self, skeleton_id):
        """
        :param skeleton_id int: Number of the skeleton of openni [1-4].
        """
        self.skeleton_id = str(skeleton_id)
        self.transformer = TransformerROS(cache_time=rospy.Duration(2.0))
        self.listener = TransformListener(self.transformer._buffer)
        self.broadcaster = TransformBroadcaster()

        self.get_bio_ik = rospy.ServiceProxy("/bio_ik/get_bio_ik",
                                             bio_ik_msgs.srv.GetIK)
        self.display_pub = rospy.Publisher("/move_group/display_planned_path",
                                           moveit_msgs.msg.DisplayTrajectory,
                                           latch=True, queue_size=10)
        self.quat_pub = rospy.Publisher(
            '/quat_debug', PoseStamped, queue_size=1)
        self.plane_pub = rospy.Publisher('/plane_debug', Marker, queue_size=1)
        self.points_pub = rospy.Publisher(
            '/pointcloud_debug', PointCloud, queue_size=1)

        self.goalhand_pub = rospy.Publisher('/goalhand', PointStamped,
                                            queue_size=1)

        self.goalelbow_pub = rospy.Publisher('/goalelbow', PointStamped,
                                            queue_size=1)
        self.lgoalhand_pub = rospy.Publisher('/lgoalhand', PointStamped,
                                            queue_size=1)

        self.lgoalelbow_pub = rospy.Publisher('/lgoalelbow', PointStamped,
                                            queue_size=1)
        rospy.sleep(1.0)
        rospy.loginfo("Initialized SkeletonToIK.")

    def do_work(self):
        ps00 = PointStamped()
        ps00.header.frame_id = 'left_shoulder' + self.skeleton_id
        lshould_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                         ps00)
        ps00.header.frame_id = 'right_shoulder' + self.skeleton_id
        rshould_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                         ps00)
        ps00.header.frame_id = 'left_hip' + self.skeleton_id
        lhip_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                      ps00)

        # looks good
        q = quaternion_from_three_points(rshould_camera.point,
                                         lshould_camera.point,
                                         lhip_camera.point)

        ts = TransformStamped()
        ts.header.stamp = rospy.Time.now()
        ts.header.frame_id = 'CameraDepth_frame'
        ts.child_frame_id = 'left_shoulder_base_frame'
        ts.transform.translation.x = lshould_camera.point.x
        ts.transform.translation.y = lshould_camera.point.y
        ts.transform.translation.z = lshould_camera.point.z
        ts.transform.rotation = Quaternion(*q)
        self.broadcaster.sendTransform(ts)
        # this frame has Z as X, -Y as Y, X as Z
        # for later using it

        ps_debug = PoseStamped()
        ps_debug.header.frame_id = 'CameraDepth_frame'
        ps_debug.pose.position = lshould_camera.point
        ps_debug.pose.orientation = Quaternion(*q)
        self.quat_pub.publish(ps_debug)
        m = Marker()
        m.type = m.TRIANGLE_LIST
        m.header.frame_id = 'CameraDepth_frame'
        m.points.append(lshould_camera.point)
        m.points.append(rshould_camera.point)
        m.points.append(lhip_camera.point)
        m.scale.x, m.scale.y, m.scale.z = 1.0, 1.0, 1.0
        m.color.a = 1.0
        m.color.r = 1.0
        # self.plane_pub.publish(m)

        pc = PointCloud()
        pc.header.frame_id = 'CameraDepth_frame'
        pc.points.append(Point32(lshould_camera.point.x,
                                 lshould_camera.point.y, lshould_camera.point.z))
        pc.points.append(Point32(rshould_camera.point.x,
                                 rshould_camera.point.y, rshould_camera.point.z))
        pc.points.append(Point32(lhip_camera.point.x,
                                 lhip_camera.point.y, lhip_camera.point.z))
        # self.points_pub.publish(pc)

        # TODO: do both arms
        # Create a 0,0,0 point on the left hand
        ps = PointStamped()
        ps.header.frame_id = 'left_hand' + self.skeleton_id
        # Get its pose in reference to its shoulder
        try:
            hand_p = self.transformer.transformPoint('left_shoulder_base_frame',
                                                     ps)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Expcetion: " + str(e))
            return

        # Same for elbow
        pselb = PointStamped()
        pselb.header.frame_id = 'left_elbow' + self.skeleton_id
        # Get its pose in reference to its shoulder
        try:
            elbow_p = self.transformer.transformPoint('left_shoulder_base_frame',
                                                      pselb)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        # TODO: adapt scale here
        # We want the distances from shoulder to elbow
        # and elbow to hand from the skeleton and the robot to be scaled
        # We could do this just once, really

        scale_forearm, scale_biceps, total_scale = self.dists_for_arms()

        hand_p.header.frame_id = 'LShoulder'
        # this frame has Z as X, -Y as Y, X as Z
        # for later using it
        hand_p.point.x, hand_p.point.y, hand_p.point.z = hand_p.point.z,  \
            hand_p.point.y, hand_p.point.x
        hand_p.point.x *= total_scale
        hand_p.point.y *= total_scale
        hand_p.point.z *= total_scale
        hand_base_link = self.transformer.transformPoint('base_link', hand_p)

        elbow_p.header.frame_id = 'LShoulder'
        elbow_p.point.x, elbow_p.point.y, elbow_p.point.z = elbow_p.point.z,  \
            elbow_p.point.y, elbow_p.point.x
        elbow_p.point.x *= total_scale
        elbow_p.point.y *= total_scale
        elbow_p.point.z *= total_scale
        elbow_base_link = self.transformer.transformPoint('base_link', elbow_p)

        self.lgoalhand_pub.publish(hand_p)
        self.lgoalelbow_pub.publish(elbow_p)

        ############## RIGHT ONE
        ps00 = PointStamped()
        ps00.header.frame_id = 'left_shoulder' + self.skeleton_id
        lshould_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                         ps00)
        ps00.header.frame_id = 'right_shoulder' + self.skeleton_id
        rshould_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                         ps00)
        ps00.header.frame_id = 'right_hip' + self.skeleton_id
        rhip_camera = self.transformer.transformPoint('CameraDepth_frame',
                                                      ps00)

        # looks good
        q = quaternion_from_three_points(rshould_camera.point,
                                         lshould_camera.point,
                                         rhip_camera.point)

        ts = TransformStamped()
        ts.header.stamp = rospy.Time.now()
        ts.header.frame_id = 'CameraDepth_frame'
        ts.child_frame_id = 'right_shoulder_base_frame'
        ts.transform.translation.x = rshould_camera.point.x
        ts.transform.translation.y = rshould_camera.point.y
        ts.transform.translation.z = rshould_camera.point.z
        ts.transform.rotation = Quaternion(*q)
        self.broadcaster.sendTransform(ts)
        # this frame has Z as X, -Y as Y, X as Z
        # for later using it

        ps_debug = PoseStamped()
        ps_debug.header.frame_id = 'CameraDepth_frame'
        ps_debug.pose.position = rshould_camera.point
        ps_debug.pose.orientation = Quaternion(*q)
        self.quat_pub.publish(ps_debug)
        m = Marker()
        m.type = m.TRIANGLE_LIST
        m.header.frame_id = 'CameraDepth_frame'
        m.points.append(lshould_camera.point)
        m.points.append(rshould_camera.point)
        m.points.append(rhip_camera.point)
        m.scale.x, m.scale.y, m.scale.z = 1.0, 1.0, 1.0
        m.color.a = 1.0
        m.color.r = 1.0
        self.plane_pub.publish(m)

        pc = PointCloud()
        pc.header.frame_id = 'CameraDepth_frame'
        pc.points.append(Point32(lshould_camera.point.x,
                                 lshould_camera.point.y, lshould_camera.point.z))
        pc.points.append(Point32(rshould_camera.point.x,
                                 rshould_camera.point.y, rshould_camera.point.z))
        pc.points.append(Point32(rhip_camera.point.x,
                                 rhip_camera.point.y, rhip_camera.point.z))
        self.points_pub.publish(pc)

        # TODO: do both arms
        # Create a 0,0,0 point on the left hand
        ps = PointStamped()
        ps.header.frame_id = 'right_hand' + self.skeleton_id
        # Get its pose in reference to its shoulder
        try:
            hand_p = self.transformer.transformPoint('right_shoulder_base_frame',
                                                     ps)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Expcetion: " + str(e))
            return

        # Same for elbow
        pselb = PointStamped()
        pselb.header.frame_id = 'right_elbow' + self.skeleton_id
        # Get its pose in reference to its shoulder
        try:
            elbow_p = self.transformer.transformPoint('right_shoulder_base_frame',
                                                      pselb)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        # TODO: adapt scale here
        # We want the distances from shoulder to elbow
        # and elbow to hand from the skeleton and the robot to be scaled
        # We could do this just once, really

        scale_forearm, scale_biceps, total_scale = self.dists_for_arms()

        hand_p.header.frame_id = 'RShoulder'
        # this frame has Z as X, -Y as Y, X as Z
        # for later using it
        hand_p.point.x, hand_p.point.y, hand_p.point.z = hand_p.point.z,  \
            hand_p.point.y, hand_p.point.x
        hand_p.point.x *= total_scale
        hand_p.point.y *= total_scale
        hand_p.point.z *= total_scale
        hand_right_base_link = self.transformer.transformPoint('base_link', hand_p)

        elbow_p.header.frame_id = 'RShoulder'
        elbow_p.point.x, elbow_p.point.y, elbow_p.point.z = elbow_p.point.z,  \
            elbow_p.point.y, elbow_p.point.x
        elbow_p.point.x *= total_scale
        elbow_p.point.y *= total_scale
        elbow_p.point.z *= total_scale
        elbow_right_base_link = self.transformer.transformPoint('base_link', elbow_p)

        self.goalhand_pub.publish(hand_p)
        self.goalelbow_pub.publish(elbow_p)


        self.do_ik_call(hand_base_link, elbow_base_link, hand_right_base_link, elbow_right_base_link)

    def dists_for_arms(self):
        ps = PointStamped()
        ps.header.frame_id = 'left_hand' + self.skeleton_id
        try:
            hand_p = self.transformer.transformPoint('left_elbow' + self.skeleton_id,
                                                     ps)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        ps2 = PointStamped()
        ps2.header.frame_id = 'l_gripper'
        try:
            hand_robot = self.transformer.transformPoint('LElbow',
                                                     ps2)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        dist_forearm_person = (abs(hand_p.point.x) + abs(hand_p.point.y) + abs(hand_p.point.z))
        dist_forearm_robot = (abs(hand_robot.point.x) + abs(hand_robot.point.y) + abs(hand_robot.point.z))

        print("forearm person: " + str(dist_forearm_person))
        print("forearm robot: " + str(dist_forearm_robot))
        scale_forearm = dist_forearm_robot / dist_forearm_person

        ps = PointStamped()
        ps.header.frame_id = 'left_elbow' + self.skeleton_id
        try:
            shoulder_p = self.transformer.transformPoint('left_shoulder' + self.skeleton_id,
                                                     ps)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        ps2 = PointStamped()
        ps2.header.frame_id = 'LElbow'
        try:
            shoulder_robot = self.transformer.transformPoint('LShoulder',
                                                     ps2)
        except LookupException as e:
            rospy.logwarn("Exception: " + str(e))
            return
        except ExtrapolationException as e:
            rospy.logwarn("Exception: " + str(e))
            return

        dist_biceps_person = (abs(shoulder_p.point.x) + abs(shoulder_p.point.y) + abs(shoulder_p.point.z))
        dist_biceps_robot = (abs(shoulder_robot.point.x) + abs(shoulder_robot.point.y) + abs(shoulder_robot.point.z))

        # print("biceps person: " + str(dist_biceps_person))
        # print("biceps robot: " + str(dist_biceps_robot))
        scale_biceps = dist_biceps_robot / dist_biceps_person

        total_scale = (dist_forearm_robot + dist_biceps_robot) / (dist_forearm_person + dist_biceps_person)

        # print "Scale forearm: " + str(scale_forearm)
        # print "Scale biceps: " + str(scale_biceps)
        print "Total scale: " + str(total_scale)
        return scale_forearm, scale_biceps, total_scale

    def do_ik_call(self, ps, pselbow, psr, psrelbow):
        request = bio_ik_msgs.msg.IKRequest()

        request.group_name = "l_arm_and_elbow"
        request.group_name = "torso_to_both_hands"

        request.timeout.secs = 0.001

        request.attempts = 1

        request.approximate = True

        request.avoid_collisions = True

        request.position_goals.append(bio_ik_msgs.msg.PositionGoal())
        request.position_goals[-1].link_name = "l_gripper"
        request.position_goals[-1].position = ps.point
        request.position_goals[-1].weight = 1.0

        request.position_goals.append(bio_ik_msgs.msg.PositionGoal())
        request.position_goals[-1].link_name = "LElbow"
        request.position_goals[-1].position = pselbow.point
        request.position_goals[-1].weight = 0.3

        request.orientation_goals.append(bio_ik_msgs.msg.OrientationGoal())
        request.orientation_goals[-1].link_name = "l_gripper"
        request.orientation_goals[-1].orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
        request.orientation_goals[-1].weight = 0.1


        request.position_goals.append(bio_ik_msgs.msg.PositionGoal())
        request.position_goals[-1].link_name = "r_gripper"
        request.position_goals[-1].position = psr.point
        request.position_goals[-1].weight = 1.0

        request.position_goals.append(bio_ik_msgs.msg.PositionGoal())
        request.position_goals[-1].link_name = "RElbow"
        request.position_goals[-1].position = psrelbow.point
        request.position_goals[-1].weight = 0.3

        request.orientation_goals.append(bio_ik_msgs.msg.OrientationGoal())
        request.orientation_goals[-1].link_name = "r_gripper"
        request.orientation_goals[-1].orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
        request.orientation_goals[-1].weight = 0.1

        response = self.get_bio_ik(request).ik_response

        print response.error_code

        display = moveit_msgs.msg.DisplayTrajectory()
        display.trajectory_start = response.solution
        display.trajectory.append(moveit_msgs.msg.RobotTrajectory())
        display.trajectory[0].joint_trajectory.points.append(
            trajectory_msgs.msg.JointTrajectoryPoint())
        display.trajectory[0].joint_trajectory.points[-1].time_from_start.secs = 0
        display.trajectory[0].joint_trajectory.points.append(
            trajectory_msgs.msg.JointTrajectoryPoint())
        display.trajectory[0].joint_trajectory.points[-1].time_from_start.secs = 1
        self.display_pub.publish(display)

    def run(self):
        r = rospy.Rate(30)
        while not rospy.is_shutdown():
            self.do_work()
            r.sleep()


if __name__ == '__main__':
    rospy.init_node('skeleton_to_ik')
    sktik = SkeletonToIK(2)
    sktik.run()