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