def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

        #too much trouble to read for now
        allow_padd = .05  #rospy.get_param(padd_name)

        joint_names = [
            'right_arm_0_joint', 'right_arm_1_joint', 'right_arm_2_joint',
            'right_arm_3_joint', 'right_arm_4_joint', 'right_arm_5_joint',
            'right_arm_6_joint'
        ]

        motion_plan_request = MotionPlanRequest()

        motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for i in range(len(joint_names)):
            motion_plan_request.goal_constraints.joint_constraints[
                i].joint_name = joint_names[i]
            motion_plan_request.goal_constraints.joint_constraints[
                i].position = 0.0
            motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_above = 0.08
            motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_below = 0.08

        motion_plan_request.goal_constraints.joint_constraints[
            0].position = 1.5
        motion_plan_request.goal_constraints.joint_constraints[
            1].position = -1.5
        motion_plan_request.goal_constraints.joint_constraints[
            5].position = 0.0

        req = GetMotionPlanRequest()
        req.motion_plan_request = motion_plan_request

        res = self.motion_planning_service.call(req)

        path = DisplayTrajectory()
        path.model_id = "right_arm"
        path.trajectory.joint_trajectory = res.trajectory.joint_trajectory
        self.traj_pub.publish(path)
示例#2
0
    def testMotionExecutionBuffer(self):
        
        global padd_name
        global extra_buffer
        
        #too much trouble to read for now
        allow_padd = .05#rospy.get_param(padd_name)
        
        joint_names = ['joint1',
                       'joint2',
                       'joint3',
                       'joint4',
                       'joint5',
                       'joint6']
        motion_plan_request = MotionPlanRequest()

        motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]

        motion_plan_request.group_name = "irp6p"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for i in range(len(joint_names)):
            motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
            motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
            motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
            motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

        motion_plan_request.goal_constraints.joint_constraints[0].position = -0.0
        motion_plan_request.goal_constraints.joint_constraints[1].position = -1.57
        motion_plan_request.goal_constraints.joint_constraints[3].position = 0.0
        motion_plan_request.goal_constraints.joint_constraints[4].position = 4.71
        motion_plan_request.goal_constraints.joint_constraints[5].position = -2.8  #1.57
        
        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
             cur_state = self.move_arm_action_client.get_state()
             if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
                cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                 break 
示例#3
0
    def testMotionExecutionBuffer(self):
        
        global padd_name
        global extra_buffer
        
        #too much trouble to read for now
        allow_padd = .05#rospy.get_param(padd_name)
        
        joint_names = ['right_arm_0_joint',
                       'right_arm_1_joint',
                       'right_arm_2_joint',
                       'right_arm_3_joint',
                       'right_arm_4_joint',
                       'right_arm_5_joint',
                       'right_arm_6_joint']

        motion_plan_request = MotionPlanRequest()

        motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for i in range(len(joint_names)):
            motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
            motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
            motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
            motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

        motion_plan_request.goal_constraints.joint_constraints[0].position = 1.5
        motion_plan_request.goal_constraints.joint_constraints[1].position = -1.5
        motion_plan_request.goal_constraints.joint_constraints[5].position = 0.0

        req = GetMotionPlanRequest()
        req.motion_plan_request = motion_plan_request

        res = self.motion_planning_service.call(req)

        path = DisplayTrajectory()
        path.model_id = "right_arm"
        path.trajectory.joint_trajectory = res.trajectory.joint_trajectory
        self.traj_pub.publish(path)
    def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

        #too much trouble to read for now
        allow_padd = .05  #rospy.get_param(padd_name)

        joint_names = [
            'right_arm_0_joint', 'right_arm_1_joint', 'right_arm_2_joint',
            'right_arm_3_joint', 'right_arm_4_joint', 'right_arm_5_joint',
            'right_arm_6_joint'
        ]
        motion_plan_request = MotionPlanRequest()

        motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for i in range(len(joint_names)):
            motion_plan_request.goal_constraints.joint_constraints[
                i].joint_name = joint_names[i]
            motion_plan_request.goal_constraints.joint_constraints[
                i].position = 0.0
            motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_above = 0.08
            motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_below = 0.08

        motion_plan_request.goal_constraints.joint_constraints[
            0].position = 1.5
        motion_plan_request.goal_constraints.joint_constraints[
            1].position = -1.8
        motion_plan_request.goal_constraints.joint_constraints[3].position = .5

        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        motion_plan_request.goal_constraints.joint_constraints[
            0].position = 0.0
        motion_plan_request.goal_constraints.joint_constraints[
            1].position = 0.0
        motion_plan_request.goal_constraints.joint_constraints[
            3].position = 0.0

        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break
    def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

        #too much trouble to read for now
        allow_padd = .05  #rospy.get_param(padd_name)

        motion_plan_request = MotionPlanRequest()

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.position_constraints = [
            PositionConstraint() for _ in range(1)
        ]
        motion_plan_request.goal_constraints.position_constraints[
            0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.position_constraints[
            0].header.frame_id = "base_link"

        motion_plan_request.goal_constraints.position_constraints[
            0].link_name = "right_arm_hand_link"
        motion_plan_request.goal_constraints.position_constraints[
            0].position.x = .65
        motion_plan_request.goal_constraints.position_constraints[
            0].position.y = -0.55
        motion_plan_request.goal_constraints.position_constraints[
            0].position.z = .9

        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_shape.type = Shape.BOX
        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_shape.dimensions = [
                float(.02) for _ in range(3)
            ]

        motion_plan_request.goal_constraints.position_constraints[
            0].constraint_region_orientation.w = 1.0
        motion_plan_request.goal_constraints.position_constraints[
            0].weight = 1.0

        motion_plan_request.goal_constraints.orientation_constraints = [
            OrientationConstraint() for _ in range(1)
        ]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.orientation_constraints[
            0].header.frame_id = "base_link"
        motion_plan_request.goal_constraints.orientation_constraints[
            0].link_name = "right_arm_hand_link"

        vals = [float() for _ in range(4)]
        vals = [-.895249, -.187638, 0.092325, .393443]
        mag = math.sqrt(vals[0] * vals[0] + vals[1] * vals[1] +
                        vals[2] * vals[2] + vals[3] * vals[3])
        vals[0] /= mag
        vals[1] /= mag
        vals[2] /= mag
        vals[3] /= mag

        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.x = vals[0]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.y = vals[1]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.z = vals[2]
        motion_plan_request.goal_constraints.orientation_constraints[
            0].orientation.w = vals[3]

        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_roll_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_pitch_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[
            0].absolute_yaw_tolerance = 0.04

        motion_plan_request.goal_constraints.orientation_constraints[
            0].weight = 1.0

        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break
示例#6
0
    def testMotionExecutionBuffer(self):
        
        global padd_name
        global extra_buffer
        
        #too much trouble to read for now
        allow_padd = .05#rospy.get_param(padd_name)
        
        motion_plan_request = MotionPlanRequest()

        motion_plan_request.group_name = "right_arm"
        motion_plan_request.num_planning_attempts = 1
        motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        motion_plan_request.planner_id = ""
        planner_service_name = "ompl_planning/plan_kinematic_path"

        motion_plan_request.goal_constraints.position_constraints = [PositionConstraint() for _ in range(1)]
        motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link"

        motion_plan_request.goal_constraints.position_constraints[0].link_name = "right_arm_hand_link"
        motion_plan_request.goal_constraints.position_constraints[0].position.x = .65
        motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.55
        motion_plan_request.goal_constraints.position_constraints[0].position.z = .9
    
        motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = Shape.BOX
        motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [float(.02) for _ in range(3)]
        
        motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
        motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0
        
        motion_plan_request.goal_constraints.orientation_constraints = [OrientationConstraint() for _ in range(1)]
        motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
        motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link"    
        motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "right_arm_hand_link"

        vals = [float() for _ in range(4)]
        vals = [-.895249, -.187638, 0.092325, .393443]
        mag = math.sqrt(vals[0]*vals[0]+vals[1]*vals[1]+vals[2]*vals[2]+vals[3]*vals[3])
        vals[0] /= mag
        vals[1] /= mag
        vals[2] /= mag
        vals[3] /= mag

        motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = vals[0]
        motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = vals[1]
        motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = vals[2]
        motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = vals[3]
        
        motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04
        motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04

        motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0

        goal = MoveArmGoal()
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"
        goal.motion_plan_request = motion_plan_request

        self.move_arm_action_client.send_goal(goal)

        while True:
             cur_state = self.move_arm_action_client.get_state()
             if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
                cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                 break