def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def run (self):
coll_obj = CollisionObject()
coll_obj.operation.operation = CollisionObjectOperation.ADD
coll_obj.id = self.name_of_the_target_object
coll_obj.header = rospy.Header()
coll_obj.header.frame_id = self.frame_id
coll_obj.poses = [self.pose_of_the_target_object]
coll_obj.shapes = [self.shape]
coll_obj.padding = self.padding
pub = rospy.Publisher('/collision_object',CollisionObject,latch=True)
rospy.loginfo('Thread publishing on /collision_object topic (%s)',self.name_of_the_target_object)
while (rospy.is_shutdown() == False) and (self.end == False):
coll_obj.header.stamp = rospy.Time.now()
try:
pub.publish(coll_obj)
except Exception, e:
rospy.logerr("Error on publishing to /collision_object topic: %s", e)
return 0
rospy.logdebug('Thread is looping')
rospy.sleep(2)
def get_virtual_table(height = 0.42):
table_msg = CollisionObject()
table_msg.id = "table"
table_msg.operation.operation = CollisionObjectOperation.ADD
table_msg.header.stamp = rospy.get_rostime()
table_msg.header.frame_id = "base_footprint"
side_box = Shape()
side_box.type = Shape.BOX
side_box.dimensions = [ 3.0, 1.0, height ]
front_box = Shape()
front_box.type = Shape.BOX
front_box.dimensions = [ 1.0, 3.0, height ]
pose = Pose()
pose.position.x = 0.0
pose.position.y = 0.0
pose.position.z = height / 2
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
l_side_pose = copy.deepcopy(pose)
l_side_pose.position.y = 0.85
r_side_pose = copy.deepcopy(pose)
r_side_pose.position.y = -0.85
front_pose = copy.deepcopy(pose)
front_pose.position.x = 0.85
table_msg.shapes = [ side_box, side_box, front_box ]
table_msg.poses = [ l_side_pose, r_side_pose, front_pose ]
return table_msg
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
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 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 test_add_convert_objects():
obj_pub = rospy.Publisher('collision_object',CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = .1
obj1.shapes[0].dimensions[2] = .75
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .375
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2";
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.CYLINDER
obj2.shapes[0].dimensions = [float() for _ in range(2)]
obj2.shapes[0].dimensions[0] = .025
obj2.shapes[0].dimensions[1] = .5
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
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
att_obj.object = obj2
r = rospy.Rate(.1)
while(True):
obj1.header.stamp = rospy.Time.now()
obj_pub.publish(obj1)
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(4)]
obj1.poses = [Pose() for _ in range(4)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .2
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = -.25
obj1.poses[1].position.z = .92
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .2
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = .2
obj1.poses[2].position.x = .8
obj1.poses[2].position.y = -.5
obj1.poses[2].position.z = .78
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[3].type = Shape.BOX
obj1.shapes[3].dimensions = [float() for _ in range(3)]
obj1.shapes[3].dimensions[0] = .2
obj1.shapes[3].dimensions[1] = .1
obj1.shapes[3].dimensions[2] = .2
obj1.poses[3].position.x = .8
obj1.poses[3].position.y = .18
obj1.poses[3].position.z = .78
obj1.poses[3].orientation.x = 0
obj1.poses[3].orientation.y = 0
obj1.poses[3].orientation.z = 0
obj1.poses[3].orientation.w = 1
obj_pub.publish(obj1)
rospy.sleep(5.0)
def add_spatula(self, arm):
spatula = CollisionObject()
spatula.id = "spatula"
spatula.header.frame_id = self.wi.world_frame
spatula.operation.operation = spatula.operation.ADD
paddle = Shape()
handle = Shape()
paddle.type = paddle.BOX
paddle.dimensions = [0.11, 0.12, 0.005]
handle.type = handle.CYLINDER
handle.dimensions = [0.02, 0.24]
paddle_pose = Pose()
handle_pose = Pose()
paddle_pose.position.y = paddle.dimensions[1] / 2.0
paddle_pose.orientation.w = 1.0
angle = np.pi / 5.0
handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
np.pi / 2.0 - angle)
handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
np.pi / 2.0 - angle)
handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)
spatula.shapes = [paddle, handle]
spatula.poses = [paddle_pose, handle_pose]
#this is the grasp transformation
pos_on_handle = handle.dimensions[1] - 0.1
inv_grasp = Transform()
grasp = RigidGrasp()
#really should be calculating this...
inv_grasp.translation.y = GRIPPER_LENGTH
inv_grasp.translation.z = pos_on_handle / 2.0
#flip 90 degrees
inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
origin = Pose()
origin.orientation.w = 1.0
grasp.transform = pose_to_transform(
gt.inverse_transform_pose(origin, g))
grasp.touch_links = [arm[0] + '_end_effector']
grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
grasp.min_approach_distance = 0
grasp.desired_approach_distance = 0.15
grasp.min_distance_from_surface = -1
spat_p = copy.deepcopy(spatula)
wtrans = Pose()
wtrans.orientation.x = np.sin(angle / 2.0)
wtrans.orientation.w = np.cos(angle / 2.0)
if self.world == -1:
wtrans.position.x = 3
wtrans.position.y = -2.8
wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['far_corner']
elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
wtrans.position.x = -1.7
wtrans.position.y = 2
wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['door_table']
else:
wtrans.position.x = 0.6
wtrans.position.y = -0.3
wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
0]
ss = ['center_table']
if self.world == -4 or self.world == -5:
wtrans.position.y = 0
rot = Quaternion()
rot.z = np.sin(np.pi / 2.0)
rot.w = np.cos(np.pi / 2.0)
wtrans.orientation = gt.multiply_quaternions(
rot, wtrans.orientation)
if self.world == -5:
wtrans.position.x = 0
for i in range(len(spat_p.poses)):
spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)
self.wi.add_object(spat_p)
return ObjectType(type="SpatulaObject",
collision_object=spatula,
parameters=ss,
numeric_parameters=paddle.dimensions +
handle.dimensions + [angle]), [grasp]
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject();
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def get_obj():
attached_obj = AttachedCollisionObject()
obj = CollisionObject()
shape = Shape()
pose = Pose()
vert = Point()
verts = []
in_verts = True
in_position = True
f = open('test.txt', 'r')
for line in f:
fields = line.split(':')
fields = [fields[i].strip() for i in range(len(fields))]
# print fields
if fields[0] == "frame_id":
obj.header.frame_id = fields[1]
elif fields[0] == "id":
obj.id = fields[1]
elif fields[0] == "operation" and fields[1] != "":
obj.operation.operation = int(fields[1])
elif fields[0] == "type":
shape.type = int(fields[1])
elif fields[0] == "triangles":
array = fields[1][1:-2]
ind = array.split(',')
inds = [int(i) for i in ind]
shape.triangles = inds
elif fields[0] == "x" and in_verts:
vert = Point()
vert.x = float(fields[1])
elif fields[0] == "y" and in_verts:
vert.y = float(fields[1])
elif fields[0] == "z" and in_verts:
vert.z = float(fields[1])
verts.append(vert)
elif fields[0] == "poses":
in_verts = False
elif fields[0] == "x" and in_position:
pose.position.x = float(fields[1])
elif fields[0] == "y" and in_position:
pose.position.y = float(fields[1])
elif fields[0] == "z" and in_position:
pose.position.z = float(fields[1])
in_position = False
elif fields[0] == "x" and not in_position:
pose.orientation.x = float(fields[1])
elif fields[0] == "y" and not in_position:
pose.orientation.y = float(fields[1])
elif fields[0] == "z" and not in_position:
pose.orientation.z = float(fields[1])
elif fields[0] == "w":
pose.orientation.w = float(fields[1])
obj.id = "graspable_object_1001"
shape.vertices = verts
obj.shapes = [shape]
obj.poses = [pose]
attached_obj.object = obj
return attached_obj
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(4)]
obj1.poses = [Pose() for _ in range(4)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .2
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = -.25
obj1.poses[1].position.z = .92
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .2
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = .2
obj1.poses[2].position.x = .8
obj1.poses[2].position.y = -.5
obj1.poses[2].position.z = .78
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[3].type = Shape.BOX
obj1.shapes[3].dimensions = [float() for _ in range(3)]
obj1.shapes[3].dimensions[0] = .2
obj1.shapes[3].dimensions[1] = .1
obj1.shapes[3].dimensions[2] = .2
obj1.poses[3].position.x = .8
obj1.poses[3].position.y = .18
obj1.poses[3].position.z = .78
obj1.poses[3].orientation.x = 0
obj1.poses[3].orientation.y = 0
obj1.poses[3].orientation.z = 0
obj1.poses[3].orientation.w = 1
obj_pub.publish(obj1)
rospy.sleep(5.0)