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 move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
move_arm_client.wait_for_server()
rospy.loginfo('connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.motion_plan_request.allowed_contacts = allowed_contacts
goal.motion_plan_request.link_padding = link_padding
goal.motion_plan_request.ordered_collision_operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('action finished: %s' % str(state))
return True
else:
rospy.loginfo('action failed: %s' % str(state))
return False
def move_arm_joint_goal(move_arm_client,
joint_names,
joint_positions,
allowed_contacts=[],
link_padding=[],
collision_operations=OrderedCollisionOperations()):
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = ARM_GROUP_NAME
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.planning_scene_diff.link_padding = link_padding
goal.operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.logerr('timed out trying to achieve joint goal')
return False
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
return True
else:
rospy.logerr('failed to achieve joint goal (returned status code %s)' % str(state))
return False
def create_move_arm_goal(self, arm):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = arm + "_arm"
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
return goal
def test_move_arm(position, orientation, frame):
rospy.loginfo("Moving the arm to x: %f y: %f z: %f r: %f p: %f y: %f", position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]);
client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
client.wait_for_server()
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame
position_constraint.link_name = "r_wrist_roll_link"
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 0.04
position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame
orientation_constraint.link_name = "r_wrist_roll_link"
orientation_constraint.orientation = quaternion_to_msg(tf.transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2]))
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
goal.disable_collision_monitoring = True
rospy.loginfo("Calling the move arm client")
state = client.send_goal_and_wait(goal, rospy.Duration(120.))
if state == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded")
else:
rospy.loginfo(state)
def _move_to_cartesian(self, coordinates):
"""
Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is
optional and can be omitted.
"""
g = MoveArmGoal()
pc = PositionConstraint()
pc.header.frame_id = "/base_link"
pc.header.stamp = rospy.Time.now()
pc.position.x = coordinates[0]
pc.position.y = coordinates[1]
pc.position.z = coordinates[2]
g.motion_plan_request.goal_constraints.position_constraints.append(pc)
oc = OrientationConstraint()
r = 0.0
p = coordinates[3] if len(coordinates) == 4 else self.pitch
y = 0.0
(qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(r, p, y)
oc.header.frame_id = "/base_link"
oc.header.stamp = rospy.Time.now()
oc.orientation.x = qx
oc.orientation.y = qy
oc.orientation.z = qz
oc.orientation.w = qw
g.motion_plan_request.goal_constraints.orientation_constraints.append(
oc)
self.move_arm_cart_server.send_goal(g)
rospy.loginfo('Sent move arm goal, waiting for result...')
self.move_arm_cart_server.wait_for_result()
rv = self.move_arm_cart_server.get_result().error_code.val
print rv
if not rv == 1:
raise Exception('Failed to move the arm to the given pose.')
def createIKGoal(pos, orientation, hand):
""" Create the message for the IK service
"""
goalA = MoveArmGoal()
goalA.motion_plan_request.group_name = hand + "_arm"
goalA.motion_plan_request.num_planning_attempts = 1
goalA.motion_plan_request.planner_id = ""
goalA.planner_service_name = "ompl_planning/plan_kinematic_path"
goalA.motion_plan_request.allowed_planning_time = rospy.Duration.from_sec(5.0);
## Set desired pose
desired_pose = createHandPose(pos, orientation, hand)
## Associate pose to goal
addGoalConstraintToMoveArmGoal(desired_pose, goalA)
return goalA
def move_goal_cb(userdata, goal):
move_goal = MoveArmGoal()
joint_names = ['jaco_joint_1' , 'jaco_joint_2', 'jaco_joint_3', 'jaco_joint_4', 'jaco_joint_5', 'jaco_joint_6' ]
move_goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
move_goal.motion_plan_request.group_name = "jaco_arm"
move_goal.motion_plan_request.num_planning_attempts = 1
move_goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
move_goal.motion_plan_request.planner_id = ""
move_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
move_goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
move_goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
move_goal.motion_plan_request.goal_constraints.joint_constraints[i].position = cur_jtang.position[i]
move_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
move_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
move_goal.motion_plan_request.goal_constraints.joint_constraints[0].position = cur_jtang.position[0] - 0.2
rospy.loginfo(rospy.get_name()+"I heard %f %f %f %f %f %f",cur_jtang.position[0],cur_jtang.position[1],cur_jtang.position[2],cur_jtang.position[3],cur_jtang.position[4],cur_jtang.position[5])
return move_goal
def move_arm_joint_goal(move_arm_client,
joint_names,
joint_positions,
allowed_contacts=[],
link_padding=[],
collision_operations=OrderedCollisionOperations()):
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = ARM_GROUP_NAME
goal.motion_plan_request.num_planning_attempts = 3
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint(j, p, 0.1, 0.1, 0.0)
for (j, p) in zip(joint_names, joint_positions)
]
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.planning_scene_diff.link_padding = link_padding
goal.operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(
rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.logerr('timed out trying to achieve joint goal')
return False
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
return True
else:
rospy.logerr(
'failed to achieve joint goal (returned status code %s)' %
str(state))
return False
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_to_cartesian(self, coordinates):
"""
Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is
optional and can be omitted.
"""
move_arm_to_goal = MoveArmGoal()
position_constraint_msg = PositionConstraint()
position_constraint_msg.header.frame_id = "/base_link"
position_constraint_msg.header.stamp = rospy.Time.now()
position_constraint_msg.position.x = coordinates[0]
position_constraint_msg.position.y = coordinates[1]
position_constraint_msg.position.z = coordinates[2]
move_arm_to_goal.motion_plan_request.goal_constraints.position_constraints.append(
position_constraint_msg)
orientation_constraint_msg = OrientationConstraint()
roll_euler = 0.0
pitch_euler = coordinates[3] if len(coordinates) == 4 else self.pitch
yaw_euler = 0.0
(quaternion_x,
quaternion_y,
quaternion_z,
quaternion_w) = tf.transformations.quaternion_from_euler(
roll_euler,
pitch_euler,
yaw_euler)
orientation_constraint_msg.header.frame_id = "/base_link"
orientation_constraint_msg.header.stamp = rospy.Time.now()
orientation_constraint_msg.orientation.x = quaternion_x
orientation_constraint_msg.orientation.y = quaternion_y
orientation_constraint_msg.orientation.z = quaternion_z
orientation_constraint_msg.orientation.w = quaternion_w
move_arm_to_goal.motion_plan_request.goal_constraints.orientation_constraints.append(
orientation_constraint_msg)
self.move_arm_cart_server.send_goal(move_arm_to_goal)
rospy.loginfo("Sent move arm goal, waiting for result...")
self.move_arm_cart_server.wait_for_result()
result_value = self.move_arm_cart_server.get_result().error_code.val
print result_value
if not result_value == 1:
raise Exception("Failed to move the arm to the given pose.")
from arm_navigation_msgs.msg import MoveArmGoal
from arm_navigation_msgs.msg import MoveArmAction
from arm_navigation_msgs.msg import PositionConstraint
from arm_navigation_msgs.msg import OrientationConstraint
from actionlib_msgs.msg import GoalStatus
if __name__ == '__main__':
rospy.init_node('arm_cartesian_goal_sender')
move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
move_arm.wait_for_server()
rospy.logout('Connected to server')
goalA = MoveArmGoal()
goalA.motion_plan_request.group_name = 'right_arm'
goalA.motion_plan_request.num_planning_attempts = 1
goalA.motion_plan_request.planner_id = ''
goalA.planner_service_name = 'ompl_planning/plan_kinematic_path'
goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
'''
Each position constraints is specified with a header, a link name,
the goal position that we want the link to reach and a tolerance region specified
using a geometric_shapes/Shape message. In this case we are trying to move the end-effector
link (r_wrist_roll_link) to the position (0.75,-0.188,0) in the torso_lift_link frame.
Note also that we fill the header with the current time (ros::Time::now()).
'''
pc = PositionConstraint()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'base_footprint'
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_" + self.arm + "_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm + "_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(
1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0] + "_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions = [0.01]
pos.constraint_region_orientation = Quaternion(0, 0, 0, 1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(
pos)
ort = OrientationConstraint()
ort.header.frame_id = goal_in.header.frame_id
ort.link_name = self.arm[0] + "_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(
ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_" + self.arm + "_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving " + self.arm +
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" % state)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning: %s" %
self.move_arm_error_dict[result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_" + self.arm + "_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move " + self.arm.capitalize() +
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded,
pos) = self.check_torso(self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
and Torso Check Failed: %s"
% self.move_arm_error_dict[result.error_code.val])
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
Failed: %s" %
self.move_arm_error_dict[result.error_code.val])
return False
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 = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
for z in range(2):
min_dist = 1000
if (z % 2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime(
"/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint(
"base_link", finger_point)
distance = math.sqrt(
math.pow(finger_point_base.point.x - .6, 2) +
math.pow(finger_point_base.point.y + .6, 2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",
finger_point_base.point.x,
finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
break
rospy.loginfo("Min dist %d is %g", z, min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd - extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state,
actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
coll)
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def main():
rospy.init_node('move_arm_action_test')
rospy.loginfo('Node for testing move_arm action')
client = actionlib.SimpleActionClient('/move_arm', MoveArmAction)
client.wait_for_server()
rospy.loginfo('Server is available, let\'s start')
goal = MoveArmGoal()
goal.motion_plan_request.group_name = 'arm'
goal.motion_plan_request.num_planning_attempts = 5
goal.motion_plan_request.planner_id = ''
goal.planner_service_name = '/ompl_planning/plan_kinematic_path'
goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.0)
# example of valid position
# - Translation: [-0.623, -0.460, 1.162]
# - Rotation: in Quaternion [0.739, -0.396, -0.533, 0.116]
# default position
# - Translation: [-0.316, -0.816, 1.593]
# - Rotation: in Quaternion [0.380, 0.153, -0.656, 0.634]
pos_const = PositionConstraint()
pos_const.header.frame_id = 'base_link'
pos_const.link_name = 'sdh_palm_link'
#pos_const.link_name = 'arm_7_link'
# default position
#pos_const.position.x = -0.316
#pos_const.position.y = -0.816
#pos_const.position.z = 1.593
pos_const.position.x = -0.623
pos_const.position.y = -0.460
pos_const.position.z = 1.162
pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_orientation.x = 0.0
pos_const.constraint_region_orientation.y = 0.0
pos_const.constraint_region_orientation.z = 0.0
pos_const.constraint_region_orientation.w = 1.0
pos_const.weight = 1.0
#pos_const.target_point_offset.x = 0.02
#pos_const.target_point_offset.y = 0.02
#pos_const.target_point_offset.z = 0.02
or_const = OrientationConstraint()
or_const.header.frame_id = 'base_link'
or_const.link_name = 'sdh_palm_link'
#or_const.link_name = 'arm_7_link'
#or_const.type = xxx
or_const.weight = 1.0
#or_const.orientation
# default orientation
#or_const.orientation.x = 0.380
#or_const.orientation.y = 0.153
#or_const.orientation.z = -0.656
#or_const.orientation.w = 0.634
or_const.orientation.x = 0.739
or_const.orientation.y = -0.396
or_const.orientation.z = -0.533
or_const.orientation.w = 0.116
or_const.absolute_pitch_tolerance = 0.04
or_const.absolute_roll_tolerance = 0.04
or_const.absolute_yaw_tolerance = 0.04
# --------------------------------------------------
goal.motion_plan_request.goal_constraints.orientation_constraints.append(
or_const)
goal.motion_plan_request.goal_constraints.position_constraints.append(
pos_const)
client.send_goal(goal)
client.wait_for_result()
res = client.get_result()
#rospy.loginfo(client.get_state())
print "Result"
print res
def main():
rospy.init_node("move_arm_action_test")
rospy.loginfo("Node for testing move_arm action")
client = actionlib.SimpleActionClient("/move_arm", MoveArmAction)
client.wait_for_server()
rospy.loginfo("Server is available, let's start")
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "arm"
goal.motion_plan_request.num_planning_attempts = 5
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "/ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.0)
# example of valid position
# - Translation: [-0.623, -0.460, 1.162]
# - Rotation: in Quaternion [0.739, -0.396, -0.533, 0.116]
# default position
# - Translation: [-0.316, -0.816, 1.593]
# - Rotation: in Quaternion [0.380, 0.153, -0.656, 0.634]
pos_const = PositionConstraint()
pos_const.header.frame_id = "base_link"
pos_const.link_name = "sdh_palm_link"
# pos_const.link_name = 'arm_7_link'
# default position
# pos_const.position.x = -0.316
# pos_const.position.y = -0.816
# pos_const.position.z = 1.593
pos_const.position.x = -0.623
pos_const.position.y = -0.460
pos_const.position.z = 1.162
pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_shape.dimensions.append(2 * 0.02)
pos_const.constraint_region_orientation.x = 0.0
pos_const.constraint_region_orientation.y = 0.0
pos_const.constraint_region_orientation.z = 0.0
pos_const.constraint_region_orientation.w = 1.0
pos_const.weight = 1.0
# pos_const.target_point_offset.x = 0.02
# pos_const.target_point_offset.y = 0.02
# pos_const.target_point_offset.z = 0.02
or_const = OrientationConstraint()
or_const.header.frame_id = "base_link"
or_const.link_name = "sdh_palm_link"
# or_const.link_name = 'arm_7_link'
# or_const.type = xxx
or_const.weight = 1.0
# or_const.orientation
# default orientation
# or_const.orientation.x = 0.380
# or_const.orientation.y = 0.153
# or_const.orientation.z = -0.656
# or_const.orientation.w = 0.634
or_const.orientation.x = 0.739
or_const.orientation.y = -0.396
or_const.orientation.z = -0.533
or_const.orientation.w = 0.116
or_const.absolute_pitch_tolerance = 0.04
or_const.absolute_roll_tolerance = 0.04
or_const.absolute_yaw_tolerance = 0.04
# --------------------------------------------------
goal.motion_plan_request.goal_constraints.orientation_constraints.append(or_const)
goal.motion_plan_request.goal_constraints.position_constraints.append(pos_const)
client.send_goal(goal)
client.wait_for_result()
res = client.get_result()
# rospy.loginfo(client.get_state())
print "Result"
print res
def execute(self, userdata):
start_time = rospy.Time.now()
self.ac = actionlib.SimpleActionClient('move_jaco_arm', MoveArmAction)
self.ac.wait_for_server()
# fill the goal pose
goalpose = userdata.move_pose
rospy.loginfo("Position 1 %f %f %f", userdata.move_pose.position.x, userdata.move_pose.position.y, userdata.move_pose.position.z)
moveArm_goal = MoveArmGoal()
moveArm_goal.motion_plan_request.group_name = "jaco_arm"
moveArm_goal.motion_plan_request.num_planning_attempts = 15
moveArm_goal.motion_plan_request.planner_id = ""
moveArm_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
moveArm_goal.motion_plan_request.allowed_planning_time = rospy.Duration(100.0)
desired_pose = SimplePoseConstraint()
desired_pose.header.frame_id = "jaco_base_link";
desired_pose.link_name = "jaco_link_6";
# desired pose get value my userdata
desired_pose.pose = goalpose
desired_pose.absolute_position_tolerance.x = 0.03;
desired_pose.absolute_position_tolerance.y = 0.03;
desired_pose.absolute_position_tolerance.z = 0.03;
desired_pose.absolute_roll_tolerance = 0.5; # 5 degree
desired_pose.absolute_pitch_tolerance = 0.5;
desired_pose.absolute_yaw_tolerance = 0.5;
addGoalConstraintToMoveArmGoal(desired_pose,moveArm_goal);
send_position[0] = desired_pose.pose.position.x
send_position[1] = desired_pose.pose.position.y
send_position[2] = desired_pose.pose.position.z
send_orientation[0] = desired_pose.pose.orientation.x
send_orientation[1] = desired_pose.pose.orientation.y
send_orientation[2] = desired_pose.pose.orientation.z
send_orientation[3] = desired_pose.pose.orientation.w
rospy.loginfo("Position 1 %f %f %f", desired_pose.pose.position.x, desired_pose.pose.position.y, desired_pose.pose.position.z)
self.ac.send_goal_and_wait(goal=moveArm_goal, execute_timeout=rospy.Duration(10))
state = self.ac.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal achieved successfully')
return 'succeeded'
elif state == GoalStatus.PREEMPTED:
rospy.loginfo('Task preempted')
return 'preempted'
elif state in [GoalStatus.RECALLED, GoalStatus.REJECTED, GoalStatus.ABORTED, GoalStatus.LOST]:
rospy.loginfo('Task aborted')
return 'aborted'
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 = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "chomp_planner_longrange/plan_path"
goal.disable_collision_monitoring = True
r = rospy.Rate(10)
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for z in range(6):
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
if (z % 2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -1.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = .22
goal.motion_plan_request.goal_constraints.joint_constraints[
1].position = .51
goal.motion_plan_request.goal_constraints.joint_constraints[
2].position = .05
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.41
goal.motion_plan_request.goal_constraints.joint_constraints[
4].position = -.06
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -.1
goal.motion_plan_request.goal_constraints.joint_constraints[
6].position = 0.0
for x in range(3):
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
r.sleep()
end_state = self.move_arm_action_client.get_state()
if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
break
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state,
actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def __move_arm(self,
arm,
position,
orientation,
frame_id,
waiting_time,
ordered_collision_operations=None,
allowed_contacts=None):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = arm
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(
waiting_time / 2.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
if arm == "right_arm":
link_name = "r_wrist_roll_link"
client = self.move_right_arm_client
elif arm == "left_arm":
link_name = "l_wrist_roll_link"
client = self.move_left_arm_client
else:
rospy.logerr("Unknown arm: %s" % arm)
return False
position_constraint.link_name = link_name
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [
tolerance, tolerance, tolerance
]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = link_name
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(
position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(
orientation_constraint)
if ordered_collision_operations is not None:
rospy.loginfo("Adding ordered collisions")
goal.operations = ordered_collision_operations
if allowed_contacts is not None:
rospy.loginfo("Adding allowed_contacts")
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.disable_collision_monitoring = False
state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
def __move_arm(self, arm, position, orientation, frame_id, waiting_time,
ordered_collision_operations = None,
allowed_contacts = None):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = arm
goal.motion_plan_request.num_planning_attempts = 2
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time/2.)
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
if arm == "right_arm":
link_name = "r_wrist_roll_link"
client = self.move_right_arm_client
elif arm == "left_arm":
link_name = "l_wrist_roll_link"
client = self.move_left_arm_client
else:
rospy.logerr("Unknown arm: %s"%arm)
return False
position_constraint.link_name = link_name
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = link_name
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
if ordered_collision_operations is not None:
rospy.loginfo("Adding ordered collisions")
goal.operations = ordered_collision_operations
if allowed_contacts is not None:
rospy.loginfo("Adding allowed_contacts")
goal.planning_scene_diff.allowed_contacts = allowed_contacts
goal.disable_collision_monitoring = False
state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
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 = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "chomp_planner_longrange/plan_path"
goal.disable_collision_monitoring = True;
r = rospy.Rate(10)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for z in range(6):
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
if(z%2==0):
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22
goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51
goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1
goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0
for x in range(3):
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
r.sleep()
end_state = self.move_arm_action_client.get_state()
if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
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 = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
for z in range(2):
min_dist = 1000
if(z%2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
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
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime("/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint("base_link",finger_point)
distance = math.sqrt(math.pow(finger_point_base.point.x-.6,2)+math.pow(finger_point_base.point.y+.6,2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",finger_point_base.point.x,finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
rospy.loginfo("Min dist %d is %g",z,min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd-extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_"+self.arm+"_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm+"_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0]+"_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions=[0.01]
pos.constraint_region_orientation = Quaternion(0,0,0,1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
ort = OrientationConstraint()
ort.header.frame_id=goal_in.header.frame_id
ort.link_name = self.arm[0]+"_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving "+self.arm+
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" %state)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning: %s"
%self.move_arm_error_dict[
result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded, pos) = self.check_torso(
self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
%state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
and Torso Check Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
return False
from arm_navigation_msgs.msg import MoveArmGoal
from arm_navigation_msgs.msg import MoveArmAction
from arm_navigation_msgs.msg import PositionConstraint
from arm_navigation_msgs.msg import OrientationConstraint
from actionlib_msgs.msg import GoalStatus
if __name__ == '__main__':
rospy.init_node('arm_cartesian_goal_sender')
move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
move_arm.wait_for_server()
rospy.logout('Connected to server')
goalA = MoveArmGoal()
goalA.motion_plan_request.group_name = 'right_arm'
goalA.motion_plan_request.num_planning_attempts = 1
goalA.motion_plan_request.planner_id = ''
goalA.planner_service_name = 'ompl_planning/plan_kinematic_path'
goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
'''
Each position constraints is specified with a header, a link name,
the goal position that we want the link to reach and a tolerance region specified
using a geometric_shapes/Shape message. In this case we are trying to move the end-effector
link (r_wrist_roll_link) to the position (0.75,-0.188,0) in the torso_lift_link frame.
Note also that we fill the header with the current time (ros::Time::now()).
'''
pc = PositionConstraint()
def main():
rospy.init_node('move_arm_action_test')
rospy.loginfo('Node for testing move_arm action')
client = actionlib.SimpleActionClient('/move_arm',MoveArmAction)
client.wait_for_server()
rospy.loginfo('Server is available, let\'s start')
goal = MoveArmGoal()
goal.motion_plan_request.group_name = 'arm'
goal.motion_plan_request.num_planning_attempts = 5
goal.motion_plan_request.planner_id = ''
goal.planner_service_name = '/ompl_planning/plan_kinematic_path'
goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.0)
pos_const = PositionConstraint()
pos_const.header.frame_id = 'base_footprint'
pos_const.link_name = 'arm_wrist_roll_link'
# default position
# rosrun tf tf_echo base_footprint arm_wrist_roll_link
#- Translation: [0.454, 0.000, 0.229]
#- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
# in RPY [0.000, 0.000, 0.000]
pos_const.position.x = 0.315077617598
pos_const.position.y = 0.260552844631
pos_const.position.z = 0.279730321177
pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX
pos_const.constraint_region_shape.dimensions.append(2*0.02)
pos_const.constraint_region_shape.dimensions.append(2*0.02)
pos_const.constraint_region_shape.dimensions.append(2*0.02)
pos_const.constraint_region_orientation.x = 0.0
pos_const.constraint_region_orientation.y = 0.0
pos_const.constraint_region_orientation.z = 0.0
pos_const.constraint_region_orientation.w = 1.0
pos_const.weight = 1.0
or_const = OrientationConstraint()
or_const.header.frame_id = 'base_footprint'
or_const.link_name = 'arm_wrist_roll_link'
or_const.weight = 1.0
# default orientation
or_const.orientation.x = -0.212992566922
or_const.orientation.y = -0.105167499832
or_const.orientation.z = 0.437894676762
or_const.orientation.w = 0.867076822132
or_const.absolute_pitch_tolerance = 0.04
or_const.absolute_roll_tolerance = 0.04
or_const.absolute_yaw_tolerance = 0.04
# --------------------------------------------------
goal.motion_plan_request.goal_constraints.orientation_constraints.append(or_const)
goal.motion_plan_request.goal_constraints.position_constraints.append(pos_const)
client.send_goal(goal)
client.wait_for_result()
res = client.get_result()
print "Result"
print res
