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()
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()
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
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
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()
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()