test_ik_with_collision_objects.py

Go to the documentation of this file.

#! /usr/bin/env python

PKG = 'pr2_arm_ik_tests'

import roslib; roslib.load_manifest(PKG)
import rospy
import sys
import unittest
import actionlib
import actionlib_msgs
import math
import random

import sensor_msgs.msg
import mapping_msgs.msg
from mapping_msgs.msg import CollisionObject, AttachedCollisionObject
from motion_planning_msgs.msg import CollisionOperation
from geometric_shapes_msgs.msg import Shape
from geometry_msgs.msg import Pose, PointStamped
from kinematics_msgs.srv import GetConstraintAwarePositionIK, GetConstraintAwarePositionIKRequest, GetKinematicSolverInfo, GetKinematicSolverInfoRequest 
from tf import TransformListener
from motion_planning_msgs.msg import JointConstraint
from planning_environment_msgs.srv import GetStateValidity,GetStateValidityRequest

class TestPr2ArmKinematicsWithCollisionObjects(unittest.TestCase):

    def generateRandomValues(self):
        ret = [float() for _ in range(len(self.min_limits))]
        for i in range(len(self.min_limits)):
            ret[i] = random.uniform(self.min_limits[i], self.max_limits[i])
        return ret

    def setUp(self):

        random.seed()

        self.coll_ik_name = '/pr2_right_arm_kinematics/get_constraint_aware_ik'
        self.env_server_name = '/environment_server/get_state_validity'
        self.ik_info = '/pr2_right_arm_kinematics/get_ik_solver_info'

        rospy.init_node('test_pr2_arm_kinematics_with_constraints')

        self.att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject,latch=True)
        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)

        rospy.wait_for_service(self.coll_ik_name)
        self.ik_service = rospy.ServiceProxy(self.coll_ik_name, GetConstraintAwarePositionIK)

        rospy.wait_for_service(self.ik_info)
        self.ik_info_service = rospy.ServiceProxy(self.ik_info, GetKinematicSolverInfo)
        
        rospy.wait_for_service(self.env_server_name)
        self.state_validity = rospy.ServiceProxy(self.env_server_name, GetStateValidity)

        self.joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']

        self.state_req = GetStateValidityRequest()
        self.state_req.robot_state.joint_state.name = self.joint_names
        self.state_req.robot_state.joint_state.position = [float(0.0) for _ in range(7)]
        self.state_req.check_collisions = True

        ik_info_req = GetKinematicSolverInfoRequest()
        ik_info_res = self.ik_info_service.call(ik_info_req)
        self.min_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))]
        self.max_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))]
        for i in range(len(ik_info_res.kinematic_solver_info.limits)):
            self.min_limits[i] = ik_info_res.kinematic_solver_info.limits[i].min_position
            self.max_limits[i] = ik_info_res.kinematic_solver_info.limits[i].max_position
        
        print len(self.min_limits), len(self.max_limits)

        self.table = CollisionObject()
         
        self.table.header.stamp = rospy.Time.now()
        self.table.header.frame_id = "base_link"
        self.table.id = "table";
        self.table.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        self.table.shapes = [Shape() for _ in range(1)]
        self.table.shapes[0].type = Shape.BOX
        self.table.shapes[0].dimensions = [float() for _ in range(3)]
        self.table.shapes[0].dimensions[0] = 1.0
        self.table.shapes[0].dimensions[1] = 1.0
        self.table.shapes[0].dimensions[2] = .05
        self.table.poses = [Pose() for _ in range(1)]
        self.table.poses[0].position.x = 1.0
        self.table.poses[0].position.y = 0
        self.table.poses[0].position.z = .5
        self.table.poses[0].orientation.x = 0
        self.table.poses[0].orientation.y = 0
        self.table.poses[0].orientation.z = 0
        self.table.poses[0].orientation.w = 1 

        self.att_box = AttachedCollisionObject()
        self.att_box.link_name = 'r_gripper_palm_link'
        self.att_box.object.header.stamp = rospy.Time.now()
        self.att_box.object.header.frame_id = "r_gripper_palm_link"
        self.att_box.object.id = "att_box.object";
        self.att_box.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        self.att_box.object.shapes = [Shape() for _ in range(1)]
        self.att_box.object.shapes[0].type = Shape.BOX
        self.att_box.object.shapes[0].dimensions = [float() for _ in range(3)]
        self.att_box.object.shapes[0].dimensions[0] = .04
        self.att_box.object.shapes[0].dimensions[1] = .04
        self.att_box.object.shapes[0].dimensions[2] = .2
        self.att_box.object.poses = [Pose() for _ in range(1)]
        self.att_box.object.poses[0].position.x = 0.12
        self.att_box.object.poses[0].position.y = 0.0
        self.att_box.object.poses[0].position.z = 0.0
        self.att_box.object.poses[0].orientation.x = 0
        self.att_box.object.poses[0].orientation.y = 0
        self.att_box.object.poses[0].orientation.z = 0
        self.att_box.object.poses[0].orientation.w = 1  
        self.att_box.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
                                    'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link']

    def tearDown(self):
        obj1 = CollisionObject()
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "all";
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        #self.obj_pub.publish(obj1)
        
        self.att_box.link_name = "all"
        self.att_box.object.id = "all"
        
        self.att_box.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        #self.att_pub.publish(self.att_box)
        
        rospy.sleep(2.0)

    def testIkWithCollisionObjects(self):

        # add table
        self.att_pub.publish(self.att_box)

        rospy.sleep(2.0)

        kin_req = GetConstraintAwarePositionIKRequest()
        kin_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        kin_req.timeout = rospy.Duration(2.0)
        kin_req.ik_request.pose_stamped.header.frame_id = 'base_link'
        kin_req.ik_request.pose_stamped.pose.position.x = .52
        kin_req.ik_request.pose_stamped.pose.position.y = -.2
        kin_req.ik_request.pose_stamped.pose.position.z = .8
        kin_req.ik_request.pose_stamped.pose.orientation.x = 0.0
        kin_req.ik_request.pose_stamped.pose.orientation.y = 0.7071
        kin_req.ik_request.pose_stamped.pose.orientation.z = 0.0
        kin_req.ik_request.pose_stamped.pose.orientation.w = 0.7071

        kin_req.ik_request.ik_seed_state.joint_state.name = self.joint_names
        kin_req.ik_request.ik_seed_state.joint_state.position = [float(0.0) for _ in range(7)]

        kin_req.ik_request.robot_state.joint_state.name = self.joint_names
        kin_req.ik_request.robot_state.joint_state.position = [float(0.0) for _ in range(7)]

        for _ in range(25):

            while(True):
                self.state_req.robot_state.joint_state.position = self.generateRandomValues()
                state_val_res = self.state_validity.call(self.state_req)
                if(state_val_res.error_code.val == state_val_res.error_code.SUCCESS):
                    break

            kin_req.ik_request.robot_state.joint_state.position = self.state_req.robot_state.joint_state.position
            kin_req.ik_request.ik_seed_state.joint_state.position = self.generateRandomValues()

            kin_res = self.ik_service.call(kin_req)
            
            self.failIf(kin_res.error_code.val != kin_res.error_code.SUCCESS)
            self.state_req.robot_state.joint_state.position = kin_res.solution.joint_state.position
            state_val_res = self.state_validity.call(self.state_req)         
            self.failIf(state_val_res.error_code.val != state_val_res.error_code.SUCCESS)
                     
if __name__ == '__main__':

    import rostest
    rostest.unitrun('test_ik_with_collision_objects', 'test_ik_with_collision_objects', TestPr2ArmKinematicsWithCollisionObjects)