def create_move_arm_goal(self):
goal = MoveArmGoal()
if self.whicharm == "r":
goal.motion_plan_request.group_name = "right_arm"
else:
goal.motion_plan_request.group_name = "left_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 move_right_arm(self, position, orientation, frame_id, waiting_time):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 10
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 / 10.);
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
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 = 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 = "r_wrist_roll_link"
# 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)
goal.disable_collision_monitoring = True
# rospy.loginfo("Goal: " + str(goal))
state = self.move_right_arm_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 = ['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
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(10):
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)
motion_plan_request = motion_planning_msgs.msg.MotionPlanRequest()
motion_plan_request.group_name = "irp6p"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(30.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.position_constraints = [motion_planning_msgs.msg.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 = "link6"
motion_plan_request.goal_constraints.position_constraints[0].position.x = .85
motion_plan_request.goal_constraints.position_constraints[0].position.y = 0.35
motion_plan_request.goal_constraints.position_constraints[0].position.z = 0.90
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 = [motion_planning_msgs.msg.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 = "link6"
vals = [float() for _ in range(4)]
vals = [-1.0, -.0, 0.0, .0]
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
#-----------------------------------------------------------------------------
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 = "link6"
motion_plan_request.goal_constraints.position_constraints[0].position.x = .85
motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.35
motion_plan_request.goal_constraints.position_constraints[0].position.z = 0.90
# motion_plan_request.path_constraints.orientation_constraints = [motion_planning_msgs.msg.OrientationConstraint() for _ in range(1)]
# motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
# motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "base_link"
# motion_plan_request.path_constraints.orientation_constraints[0].link_name = "link6"
# motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = -1.0
# motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0
# motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0
# motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 0.0
# motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 1.5
# motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 1.5
# motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = 6.04
# motion_plan_request.path_constraints.orientation_constraints[0].type = motion_planning_msgs.msg.OrientationConstraint.HEADER_FRAME
# motion_plan_request.path_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
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
# too much trouble to read for now
allow_padd = 0.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.0)
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 = 0.22
goal.motion_plan_request.goal_constraints.joint_constraints[1].position = 0.51
goal.motion_plan_request.goal_constraints.joint_constraints[2].position = 0.05
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -0.06
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.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)
from move_arm_msgs.msg import MoveArmAction
from motion_planning_msgs.msg import PositionConstraint
from motion_planning_msgs.msg import OrientationConstraint
from geometric_shapes_msgs.msg import Shape
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.)
pc = PositionConstraint()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'torso_lift_link'
pc.link_name = 'r_wrist_roll_link'
pc.position.x = 0.75
pc.position.y = -0.188
pc.position.z = 0
from move_arm_msgs.msg import MoveArmGoal
from motion_planning_msgs.msg import JointConstraint
from actionlib_msgs.msg import GoalStatus
if __name__ == '__main__':
import hrl_lib.transforms as tr
rospy.init_node('move_arm_joint_goal_test')
move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
move_arm.wait_for_server()
rospy.loginfo('Connected to server')
goalB = MoveArmGoal()
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'
]
goalB.motion_plan_request.group_name = 'right_arm'
goalB.motion_plan_request.num_planning_attempts = 1
goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goalB.motion_plan_request.planner_id = ''
goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'
import roslib
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)
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 = mapping_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 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
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 = mapping_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)