def get_fingertip(hand_pose):
"""
Calculates the point between the finger tips, where objects will be hold.
:param hand_pose: hand pose
:type: PoseStamped
:return: point between fingers
:type: PointStamped
"""
grasp_point = PointStamped()
grasp_point.point = set_vector_length(hand_length + finger_length, Point(1, 0, 0))
grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point)
grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point)
grasp_point.header.frame_id = hand_pose.header.frame_id
return grasp_point
def get_fingertip(hand_pose):
"""
Calculates the point between the finger tips, where objects will be hold.
:param hand_pose: hand pose
:type: PoseStamped
:return: point between fingers
:type: PointStamped
"""
grasp_point = PointStamped()
grasp_point.point = set_vector_length(hand_length + finger_length,
Point(1, 0, 0))
grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point)
grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point)
grasp_point.header.frame_id = hand_pose.header.frame_id
return grasp_point
def get_center_of_mass(self, collision_object):
"""
Calculates the centre of a collision object.
:param collision_object: CollisionObject
:type: CollisionObject
:return: centre of mass
:type: PointStamped
"""
p = PointStamped()
p.header.frame_id = "/odom_combined"
for pose in collision_object.primitive_poses:
p.point = add_point(p.point, pose.position)
p.point = multiply_point(1.0 / len(collision_object.primitive_poses), p.point)
return p
def get_center_of_mass(self, collision_object):
"""
Calculates the centre of a collision object.
:param collision_object: CollisionObject
:type: CollisionObject
:return: centre of mass
:type: PointStamped
"""
p = PointStamped()
p.header.frame_id = "/odom_combined"
for pose in collision_object.primitive_poses:
p.point = add_point(p.point, pose.position)
p.point = multiply_point(1.0 / len(collision_object.primitive_poses),
p.point)
return p
def get_base_origin(self):
"""
:return: The centre of the arm's base
:type: PointStamped
"""
current_pose = self.__base_group.get_current_joint_values()
result = PointStamped()
result.header.frame_id = "/odom_combined"
result.point = Point(current_pose[0], current_pose[1], 0)
return result
def get_base_origin(self):
"""
:return: The centre of the arm's base
:type: PointStamped
"""
current_pose = self.__base_group.get_current_joint_values()
result = PointStamped()
result.header.frame_id = "/odom_combined"
result.point = Point(current_pose[0], current_pose[1], 0)
return result
def test_task3(mani):
dest = PointStamped()
dest.header.frame_id = "/odom_combined"
dest.point = Point(0.85, -0.85, 0)
mani.place_and_move(dest)
def test_task3(mani):
dest = PointStamped()
dest.header.frame_id = "/odom_combined"
dest.point = Point(0.85, -0.85, 0)
mani.place_and_move(dest)
