def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0, 0)
co.primitive_poses[0].orientation = Quaternion(0, 0, 0, 1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0, 0)
co.primitive_poses[1].orientation = Quaternion(0, 0, 0, 1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0, 0)
co.primitive_poses[2].orientation = Quaternion(0, 0, 0, 1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1, 0, 0)
p.pose.orientation = euler_to_quaternion(0, 0, 0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def close_gripper(self, object=None, grasp_point=None):
"""
Closes the gripper completely or far enough to hold the object, when one is given
:param object: Object that will be grasped.
:type: CollisionObject
:return: success of the movement
:type: bool
"""
rospy.logdebug("Closing Gripper")
if type(object) is CollisionObject:
self.__gripper_group.attach_object(object.id, "gp",
["gp", "finger1", "finger2"])
rospy.sleep(1.0)
id = get_grasped_part(object, grasp_point)[1]
# id = min(range(len(object.primitives)), key=lambda i: min(object.primitives[i].dimensions))
# TODO: only works for cubes and cylinders and only "sometimes" for object compositions
if object.primitives[id].type == shape_msgs.msg.SolidPrimitive.BOX:
length = min(object.primitives[id].dimensions)
self.__gripper_group.set_joint_value_target(
[-(length / 2), length / 2])
elif object.primitives[
id].type == shape_msgs.msg.SolidPrimitive.CYLINDER:
radius = object.primitives[id].dimensions[
shape_msgs.msg.SolidPrimitive.CYLINDER_RADIUS]
if radius >= gripper_max_pose:
rospy.logdebug("Object is too big!")
return False
self.__gripper_group.set_joint_value_target(
[-radius + 0.005, radius - 0.005])
else:
self.__gripper_group.set_joint_value_target([0.0, 0.0])
path = self.__gripper_group.plan()
return self.__manService.move(path)
def close_gripper(self, object=None, grasp_point=None):
"""
Closes the gripper completely or far enough to hold the object, when one is given
:param object: Object that will be grasped.
:type: CollisionObject
:return: success of the movement
:type: bool
"""
rospy.logdebug("Closing Gripper")
if type(object) is CollisionObject:
self.__gripper_group.attach_object(object.id, "gp", ["gp", "finger1", "finger2"])
rospy.sleep(1.0)
id = get_grasped_part(object, grasp_point)[1]
# id = min(range(len(object.primitives)), key=lambda i: min(object.primitives[i].dimensions))
# TODO: only works for cubes and cylinders and only "sometimes" for object compositions
if object.primitives[id].type == shape_msgs.msg.SolidPrimitive.BOX:
length = min(object.primitives[id].dimensions)
self.__gripper_group.set_joint_value_target([-(length / 2), length / 2])
elif object.primitives[id].type == shape_msgs.msg.SolidPrimitive.CYLINDER:
radius = object.primitives[id].dimensions[shape_msgs.msg.SolidPrimitive.CYLINDER_RADIUS]
if radius >= gripper_max_pose:
rospy.logdebug("Object is too big!")
return False
self.__gripper_group.set_joint_value_target([-radius + 0.005, radius - 0.005])
else:
self.__gripper_group.set_joint_value_target([0.0, 0.0])
path = self.__gripper_group.plan()
return self.__manService.move(path)
def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
