test_constrained_planning.py

Go to the documentation of this file.

#! /usr/bin/env python

PKG = 'arm_navigation_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
from motion_planning_msgs.msg import CollisionOperation, DisplayTrajectory
from geometric_shapes_msgs.msg import Shape
from geometry_msgs.msg import Pose, PointStamped, Quaternion
from motion_planning_msgs.srv import GetMotionPlan, GetMotionPlanRequest
from tf import TransformListener
from motion_planning_msgs.msg import JointConstraint, OrientationConstraint, PositionConstraint
from planning_environment_msgs import planning_environment_msgs_utils
from kinematics_msgs.srv import GetConstraintAwarePositionIK, GetConstraintAwarePositionIKRequest, GetKinematicSolverInfo, GetKinematicSolverInfoRequest 
from planning_environment_msgs.srv import GetStateValidity,GetStateValidityRequest, GetJointTrajectoryValidity, GetJointTrajectoryValidityRequest

class TestConstrainedPlanning(unittest.TestCase):

    def generateRandomUnitQuaternion(self):
        orientation = Quaternion()
        orientation.x = random.uniform(-1.0, 1.0)
        orientation.y = random.uniform(-1.0, 1.0)
        orientation.z = random.uniform(-1.0, 1.0)
        orientation.w = random.uniform(-1.0, 1.0)

        mag = math.sqrt(math.pow(orientation.x,2.0)+math.pow(orientation.y,2.0)+math.pow(orientation.z,2.0)+math.pow(orientation.w,2.0))
        orientation.x /= mag
        orientation.y /= mag
        orientation.z /= mag
        orientation.w /= mag
        
        return orientation

    def getGraspQuaternion(self, grasp_name):
        
        orientation = Quaternion()

        if(grasp_name == 'top'):
            orientation.x = 0.0
            orientation.y = 0.7071
            orientation.z = 0.0
            orientation.w = 0.7071
        else:
            orientation.x = 0.0
            orientation.y = 0.0
            orientation.z = 0.0
            orientation.w = 1.0
    
        return orientation

    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 generateValidRandomRobotState(self):
        
        valid_state_req = GetStateValidityRequest()
        valid_state_req.robot_state.joint_state.name = self.joint_names
        valid_state_req.check_collisions = True
        
        while(True):
            valid_state_req.robot_state.joint_state.position = self.generateRandomValues()
            state_val_res = self.state_validity_service.call(valid_state_req)
            if(state_val_res.error_code.val == state_val_res.error_code.SUCCESS):
                return valid_state_req.robot_state

    def generateRandomValidConstrainedState(self, quat):

        kin_req = GetConstraintAwarePositionIKRequest()
        kin_req.ik_request.ik_link_name = 'r_wrist_roll_link'

        kin_req.ik_request.ik_seed_state.joint_state.name = self.joint_names
        kin_req.ik_request.ik_seed_state.joint_state.position = self.generateRandomValues()
        kin_req.ik_request.robot_state = self.generateValidRandomRobotState()

        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.orientation = quat

        while(True):
            kin_req.ik_request.pose_stamped.pose.position.x = random.uniform(self.x_workspace_min, self.x_workspace_max)
            kin_req.ik_request.pose_stamped.pose.position.y = random.uniform(self.y_workspace_min, self.y_workspace_max)
            kin_req.ik_request.pose_stamped.pose.position.z = random.uniform(self.z_workspace_min, self.z_workspace_max)

            kin_res = self.ik_service.call(kin_req)

            if(kin_res.error_code.val == kin_res.error_code.SUCCESS):
                break

        return (kin_res.solution,kin_req.ik_request.pose_stamped)

    def setUp(self):

        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.coll_ik_name = '/pr2_right_arm_kinematics/get_constraint_aware_ik'
        self.env_server_name = '/environment_server/get_state_validity'
        self.ik_info_name = '/pr2_right_arm_kinematics/get_ik_solver_info'
        self.motion_planning_name = '/ik_constrained_planner/plan_path'
        self.check_trajectory_name = '/environment_server/get_trajectory_validity'

        self.x_workspace_min = -.1
        self.x_workspace_max = 1.0
        
        self.y_workspace_min = -1.0
        self.y_workspace_max = .3
        
        self.z_workspace_min = 1.0
        self.z_workspace_max = 0.0

        rospy.init_node('test_constrained_planning')

        self.trajectory_pub = rospy.Publisher('display_path', DisplayTrajectory)        

        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_name)
        self.ik_info_service = rospy.ServiceProxy(self.ik_info_name, GetKinematicSolverInfo)
        
        rospy.wait_for_service(self.env_server_name)
        self.state_validity_service = rospy.ServiceProxy(self.env_server_name, GetStateValidity)

        rospy.wait_for_service(self.check_trajectory_name)
        self.check_trajectory_service = rospy.ServiceProxy(self.check_trajectory_name, GetJointTrajectoryValidity)

        rospy.wait_for_service(self.motion_planning_name)
        self.motion_planning_service = rospy.ServiceProxy(self.motion_planning_name, GetMotionPlan)

        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

        

    def tearDown(self):

        print 'teardown'

    def testIkConstrainedPlanner(self):

        for _ in range(25):

            quat = self.generateRandomUnitQuaternion()

            (start_state,start_pose) = self.generateRandomValidConstrainedState(quat)
            (goal_state, goal_pose) = self.generateRandomValidConstrainedState(quat)

            motion_goal = GetMotionPlanRequest()
            motion_goal.motion_plan_request.group_name = "right_arm"
            motion_goal.motion_plan_request.num_planning_attempts = 1
            motion_goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
            motion_goal.motion_plan_request.planner_id = ""
            
            motion_goal.motion_plan_request.start_state = start_state

            motion_goal.motion_plan_request.goal_constraints.position_constraints = [PositionConstraint() for _ in range(1)]
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = 'base_link'
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].link_name = 'r_wrist_roll_link'
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].position = goal_pose.pose.position
            
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = Shape.BOX;
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [float(.02) for _ in range(3)]

            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
            motion_goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;            
            
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints = [OrientationConstraint() for _ in range(1)]
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link"
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link"
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation = goal_pose.pose.orientation
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04
            motion_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04

            motion_goal.motion_plan_request.path_constraints.orientation_constraints = [OrientationConstraint() for _ in range(1)]
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link"
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link"
            
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].orientation = goal_pose.pose.orientation

            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].type = OrientationConstraint.HEADER_FRAME
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.1
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.1
            motion_goal.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = math.pi
            
            try:
                motion_res = self.motion_planning_service.call(motion_goal)
            except rospy.ServiceException, e:
                self.fail('motion planning failed')

            #self.failIf(motion_res.error_code.val != motion_res.error_code.SUCCESS, motion_res.error_code.val)
            
            trajectory_check_req = GetJointTrajectoryValidityRequest()
            trajectory_check_req.robot_state = motion_res.robot_state
            trajectory_check_req.trajectory = motion_res.trajectory.joint_trajectory
            trajectory_check_req.check_collisions = True
            trajectory_check_req.check_joint_limits = True
            trajectory_check_req.check_path_constraints = True
            trajectory_check_req.check_full_trajectory = True
            trajectory_check_req.goal_constraints = motion_goal.motion_plan_request.goal_constraints
            trajectory_check_req.path_constraints = motion_goal.motion_plan_request.path_constraints

            trajectory_check_res = self.check_trajectory_service.call(trajectory_check_req)
            
            self.failIf(trajectory_check_res.error_code.val != trajectory_check_res.error_code.SUCCESS)

            disp = DisplayTrajectory()
            disp.model_id = "right_arm"
            disp.trajectory.joint_trajectory = trajectory_check_req.trajectory
            disp.robot_state = trajectory_check_req.robot_state
            self.trajectory_pub.publish(disp)

            rospy.sleep(2.0)

if __name__ == '__main__':

    import rostest
    rostest.unitrun('test_constrained_planning', 'test_constrained_planning', TestConstrainedPlanning)