Esempio n. 1
0
def move_arm_ompl(x, y, z, ox=0.0, oy=0.0, oz=0.0, ow=1.0, arm_service = "move_right_arm", shoulder_tolerance=(0.5, 0.3), frame="odom_combined"):
    client = actionlib.SimpleActionClient(arm_service, MoveArmAction)
    client.wait_for_server()
    
    goal = MoveArmGoal()
    goal.disable_ik = True
    goal.planner_service_name = "ompl_planning/plan_kinematic_path"
    goal.motion_plan_request.group_name = "right_arm"
    goal.motion_plan_request.num_planning_attempts = 10
    goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
    goal.motion_plan_request.planner_id = ""

    addPositionConstraint(goal, x, y, z, frame)
    addOrientationConstraint(goal, ox, oy, oz, ow, frame)
    
    constraint = JointConstraint()
    constraint.joint_name = "r_shoulder_lift_joint"
    constraint.position = -0.05
    constraint.tolerance_above = shoulder_tolerance[0]
    constraint.tolerance_below = shoulder_tolerance[1]
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
    
    
    client.send_goal(goal, done_cb=move_arm_completed_cb)
    print client.wait_for_result()
Esempio n. 2
0
def move_arm_ompl(x,
                  y,
                  z,
                  ox=0.0,
                  oy=0.0,
                  oz=0.0,
                  ow=1.0,
                  arm_service="move_right_arm",
                  shoulder_tolerance=(0.5, 0.3),
                  frame="odom_combined"):
    client = actionlib.SimpleActionClient(arm_service, MoveArmAction)
    client.wait_for_server()

    goal = MoveArmGoal()
    goal.disable_ik = True
    goal.planner_service_name = "ompl_planning/plan_kinematic_path"
    goal.motion_plan_request.group_name = "right_arm"
    goal.motion_plan_request.num_planning_attempts = 10
    goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
    goal.motion_plan_request.planner_id = ""

    addPositionConstraint(goal, x, y, z, frame)
    addOrientationConstraint(goal, ox, oy, oz, ow, frame)

    constraint = JointConstraint()
    constraint.joint_name = "r_shoulder_lift_joint"
    constraint.position = -0.05
    constraint.tolerance_above = shoulder_tolerance[0]
    constraint.tolerance_below = shoulder_tolerance[1]
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(
        constraint)

    client.send_goal(goal, done_cb=move_arm_completed_cb)
    print client.wait_for_result()
Esempio n. 3
0
def getJointConstraints(goal):
    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"
    ]
    joint_constraints = []
    for joint_name in joint_names:
        constraint = JointConstraint()
        constraint.joint_name = joint_name
        constraint.position = 0
        constraint.tolerance_above = 0.1
        constraint.tolerance_below = 0.1
        constraint.weight = 1.0
        joint_constraints.append(constraint)

    joint_constraints[0].position = -2
    joint_constraints[3].position = -0.2
    joint_constraints[5].position = -0.15
    goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
Esempio n. 4
0
def getJointConstraints(goal):
    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"]
    joint_constraints = []
    for joint_name in joint_names:
        constraint = JointConstraint()
        constraint.joint_name = joint_name
        constraint.position = 0
        constraint.tolerance_above = 0.1
        constraint.tolerance_below = 0.1
        constraint.weight = 1.0
        joint_constraints.append(constraint)
    
    joint_constraints[0].position = -2
    joint_constraints[3].position = -0.2
    joint_constraints[5].position = -0.15
    goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
Esempio n. 5
0
def move_arm_ompl(x, y, z):
    service = "move_right_arm"
    client = actionlib.SimpleActionClient(service, MoveArmAction)
    client.wait_for_server()
    
    goal = MoveArmGoal()
    goal.disable_ik = True
    goal.planner_service_name = "ompl_planning/plan_kinematic_path"
    goal.motion_plan_request.group_name = "right_arm"
    goal.motion_plan_request.num_planning_attempts = 5
    goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
    goal.motion_plan_request.planner_id = ""

    addPositionConstraint(goal, x, y, z)
    addOrientationConstraint(goal)
    
    '''
    constraint = JointConstraint()
    constraint.joint_name = "r_upper_arm_roll_joint"
    constraint.position = -2
    constraint.tolerance_above = 0.1
    constraint.tolerance_below = 0.1
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
    
    '''
    constraint = JointConstraint()
    constraint.joint_name = "r_shoulder_lift_joint"
    constraint.position = 0.4
    constraint.tolerance_above = 0.2
    constraint.tolerance_below = 0.2
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
    
    
    client.send_goal(goal, done_cb=move_arm_completed_cb)
    client.wait_for_result()
Esempio n. 6
0
def move_arm_ompl(x, y, z):
    service = "move_right_arm"
    client = actionlib.SimpleActionClient(service, MoveArmAction)
    client.wait_for_server()

    goal = MoveArmGoal()
    goal.disable_ik = True
    goal.planner_service_name = "ompl_planning/plan_kinematic_path"
    goal.motion_plan_request.group_name = "right_arm"
    goal.motion_plan_request.num_planning_attempts = 5
    goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5)
    goal.motion_plan_request.planner_id = ""

    addPositionConstraint(goal, x, y, z)
    addOrientationConstraint(goal)
    '''
    constraint = JointConstraint()
    constraint.joint_name = "r_upper_arm_roll_joint"
    constraint.position = -2
    constraint.tolerance_above = 0.1
    constraint.tolerance_below = 0.1
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint)
    
    '''
    constraint = JointConstraint()
    constraint.joint_name = "r_shoulder_lift_joint"
    constraint.position = 0.4
    constraint.tolerance_above = 0.2
    constraint.tolerance_below = 0.2
    constraint.weight = 1.0
    goal.motion_plan_request.goal_constraints.joint_constraints.append(
        constraint)

    client.send_goal(goal, done_cb=move_arm_completed_cb)
    client.wait_for_result()
Esempio n. 7
0
    def move_arm_joint(self, arm, joint_angles):

        self.set_planning_scene()

        rospy.loginfo(
            "asking move_arm to send the %s arm to go to angles: %s" %
            (arm, self.pplist(joint_angles)))

        goal = self.create_move_arm_goal(arm)

        #add the joint angles as a joint constraint
        for (joint_name, joint_angle) in zip(self.joint_names[arm],
                                             joint_angles):
            joint_constraint = JointConstraint()
            joint_constraint.joint_name = joint_name
            joint_constraint.position = joint_angle
            joint_constraint.tolerance_below = .1
            joint_constraint.tolerance_above = .1
            goal.motion_plan_request.goal_constraints.joint_constraints.append(
                joint_constraint)

        #send the goal off to move arm
        result = self.send_move_arm_goal(arm, goal)
        return result