class RandPoseGenerator(EventState):
'''
Periodically generate geometry_msgs/PoseStamped messge in given parallelepiped.
Frames for parallelepiped is defined and the frame in which message is published can be different.
-- topic string Topic where PoseStamped message should be published.
-- duration float How long this state will be executed (seconds).
-- interval float[2] Array of floats, maximal and minimal interval between movements.
-- minXYZ float[3] Max values for x, y, z coordinates.
-- maxXYZ float[3] Min values for x, y, z coordinates.
-- frame_xyz string Coordinate frame of parallelepiped.
-- frame_out string Frame in which pose is publised.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
'''
def __init__(self,
topic='motion/controller/look_at/in_pose_ref',
duration=120,
interval=[3, 5],
maxXYZ=[1, 0.3, 0.5],
minXYZ=[1.0, -0.3, 0.0],
frame_xyz='base_link',
frame_out='odom_combined'):
super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
if not isinstance(topic, str):
raise ValueError("Topic name must be string.")
if not isinstance(duration, (int, float)) or duration <= 0:
raise ValueError("Duration must be positive number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(minXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in minXYZ]):
raise ValueError("minXYZ: list of three numbers was expected.")
if len(maxXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in maxXYZ]):
raise ValueError("maxXYZ: list of three numbers was expected.")
if not isinstance(frame_xyz, str) or not isinstance(frame_out, str):
raise ValueError("Frame names must be string.")
self._topic = topic
self._duration = Duration.from_sec(duration)
self._interval = interval
self._minXYZ = minXYZ
self._maxXYZ = maxXYZ
self._frame_in = frame_xyz
self._frame_out = frame_out
# create proxies
self._publisher = ProxyPublisher({self._topic: PoseStamped})
self._tf_listener = ProxyTransformListener().listener()
self._tf_listener.waitForTransform(self._frame_out, self._frame_in,
rospy.Time(), rospy.Duration(1))
if not self._tf_listener.canTransform(self._frame_out, self._frame_in,
rospy.Time(0)):
raise ValueError(
"Unable to perform transform between frames %s and %s." %
(self._frame_out, self._frame_in))
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# error in enter hook
self._error = False
def on_enter(self, userdata):
self._error = False
# set start timestamp
self._start_timestamp = Time.now()
self._movement_timestamp = Time.now()
Logger.loginfo('Start random pose generation, topic `%s`.' %
self._topic)
def execute(self, userdata):
if self._error:
return 'failure'
time_now = Time.now()
# check if time elasped
if time_now - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if time_now - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = time_now
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = PoseStamped()
msg.header.frame_id = self._frame_in
msg.header.stamp = Time(0.0)
# random position
msg.pose.position.x = random.uniform(self._minXYZ[0],
self._maxXYZ[0])
msg.pose.position.y = random.uniform(self._minXYZ[1],
self._maxXYZ[1])
msg.pose.position.z = random.uniform(self._minXYZ[2],
self._maxXYZ[2])
# transform to output frame
try:
# transform
self._pose = self._tf_listener.transformPose(
self._frame_out, msg)
except tf.Exception as e:
Logger.logwarn('Unable to transform from %s to %s: %s' %
(self._frame_in, self._frame_out, e))
return 'failure'
# publish pose
msg.header.stamp = self._movement_timestamp
self._publisher.publish(self._topic, msg)
def on_exit(self, userdata):
Logger.loginfo('Done random pose generation.')
class CalculateCartesianPathState(EventState):
'''
Calculates the trajectory for a cartesian path to the given goal endeffector pose.
-- move_group string Name of the move group to be used for planning.
-- ignore_collisions bool Ignore collision, use carefully.
># eef_pose PoseStamped Target pose of the endeffector.
#> joint_trajectory JointTrajectory Calculated joint trajectory.
#> plan_fraction float Fraction of the planned path.
<= planned Found a plan to the target.
<= failed Failed to find a plan to the given target.
'''
def __init__(self, move_group, ignore_collisions = False):
'''
Constructor
'''
super(CalculateCartesianPathState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['eef_pose'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._topic = '/compute_cartesian_path'
self._srv = ProxyServiceCaller({self._topic: GetCartesianPath})
self._tf = ProxyTransformListener().listener()
self._move_group = move_group
self._result = None
self._failed = False
self._ignore_collisions = ignore_collisions
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._result.error_code.val == MoveItErrorCodes.SUCCESS and self._result.fraction > .2:
userdata.plan_fraction = self._result.fraction
userdata.joint_trajectory = self._result.solution.joint_trajectory
Logger.loginfo('Successfully planned %.2f of the path' % self._result.fraction)
return 'planned'
elif self._result.error_code.val == MoveItErrorCodes.SUCCESS:
Logger.logwarn('Only planned %.2f of the path' % self._result.fraction)
self._failed = True
return 'failed'
else:
Logger.logwarn('Failed to plan trajectory: %d' % self._result.error_code.val)
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
request = GetCartesianPathRequest()
request.group_name = self._move_group
request.avoid_collisions = not self._ignore_collisions
request.max_step = 0.05
request.header = userdata.eef_pose.header
request.waypoints.append(userdata.eef_pose.pose)
now = rospy.Time.now()
try:
self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)
c = OrientationConstraint()
c.header.frame_id = 'base_link'
c.header.stamp = now
c.link_name = 'gripper_cam_link'
c.orientation.x = q[0]
c.orientation.y = q[1]
c.orientation.z = q[2]
c.orientation.w = q[3]
c.weight = 1
c.absolute_x_axis_tolerance = 0.1
c.absolute_y_axis_tolerance = 0.1
c.absolute_z_axis_tolerance = 0.1
#request.path_constraints.orientation_constraints.append(c)
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Exception while calculating path:\n%s' % str(e))
self._failed = True
class HeadIK:
'''
Implements SweetieBot head and eyes invese kinematics. Meant to be run in FlexBe state enviroment.
'''
def __init__(self):
'''
Get tf proxy and neessary transform.
:raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise
'''
# get transform listener singlenton
self._tf = ProxyTransformListener().listener()
# wait for pose publication
self._tf.waitForTransform('bone51', 'base_link', rospy.Time(),
rospy.Duration(10.0))
# get neck base translation vector in base_link frame
(trans, rot) = self._tf.lookupTransform('base_link', 'bone51',
rospy.Time())
self._t5051 = trans
# get translation from bone51 to bone52
(trans, rot) = self._tf.lookupTransform('bone51', 'bone52',
rospy.Time())
self._t5152 = trans
def pointDirectionToHeadPose(self,
point_stamped,
neck_angle,
side_angle,
limit_joints=True):
'''
Calculate Proto2 head pose so it is oriented toward the given point.
:param point_stamped: geometry_msg.msg.PointStamped point to look at.
:param neck_angle: joint51 desired angle in radians.
:param side_angle: joint54 desired angle in radians.
:param limit_joints: limit joints positions to prevent self collisions.
:return: sensor_msg.msg.JointState head pose (joint ranges are not checked). Return `None` if transform is not available.
'''
joints = JointState()
joints.header = Header()
joints.header.stamp = rospy.Time.now()
# set known fields
joints.name = ['joint51', 'joint52', 'joint53', 'joint54']
joints.position = [neck_angle, 0.0, 0.0, side_angle]
if limit_joints:
joints.position = self.limitHeadJoints51to54(joints.position)
neck_angle = joints.position[0]
side_angle = joints.position[3]
try:
# convert point coordinates to base_link frame
point_stamped = self._tf.transformPoint(
'base_link',
PointStamped(
header=Header(frame_id=point_stamped.header.frame_id),
point=point_stamped.point))
except tf.Exception as e:
Logger.logwarn('Cannot transform to base_link:\n%s' % str(e))
return None
# move to neck base
direction = [
point_stamped.point.x, point_stamped.point.y,
point_stamped.point.z, 1.0
]
upward = [0.0, 0.0, 1.0, 1.0]
direction[:3] = numpy.subtract(direction[:3], self._t5051)
# rotate to neck_angle around Ox to transform it to desired bone51 frame.
direction = numpy.dot(rotation_matrix(neck_angle, (0, 1, 0)),
direction)
upward = numpy.dot(rotation_matrix(neck_angle, (0, 1, 0)), upward)
# move origin to bone52
direction[:3] = numpy.subtract(direction[:3], self._t5152)
# calculate rotation angles
print(upward)
joints.position[1] = math.atan2(direction[1], direction[0])
upward = numpy.dot(rotation_matrix(joints.position[1], (0, 0, 1)),
upward)
print(upward)
joints.position[2] = math.atan2(
direction[2], math.sqrt(direction[0]**2 + direction[1]**2))
upward = numpy.dot(rotation_matrix(joints.position[2], (0, 1, 0)),
upward)
print(upward)
joints.position[3] = math.atan2(upward[1], upward[2]) + side_angle
# limit joints
if limit_joints:
joints.position = self.limitHeadJoints51to54(joints.position)
return joints
def pointDirectionToEyesPose(self, point_stamped):
'''
Calculate Proto2 eyes angles for deprecated eyes module.
:param point_stamped: geometry_msg.msg.PointStamped point to look at.
:return: sensor_msg.msg.JointState head pose (joint ranges are not checked). Return `None` if transform is not available.
'''
joints = JointState()
joints.header = Header()
joints.header.stamp = rospy.Time.now()
# set known fields
joints.name = ['eyes_pitch', 'eyes_yaw']
joints.position = [0.0, 0.0]
try:
# convert point coordinates to base_link frame
point_stamped = self._tf.transformPoint(
'bone54',
PointStamped(
header=Header(frame_id=point_stamped.header.frame_id),
point=point_stamped.point))
except tf.Exception as e:
Logger.logwarn('Cannot transform to bone54:\n%s' % str(e))
return None
# calculate angles
direction = [
point_stamped.point.x, point_stamped.point.y, point_stamped.point.z
]
joints.position[0] = math.atan2(
point_stamped.point.z,
math.sqrt(point_stamped.point.x**2 + point_stamped.point.y**2))
joints.position[1] = -math.atan2(
point_stamped.point.y,
math.sqrt(point_stamped.point.x**2 + point_stamped.point.z**2))
# limit joints position
joints.position = list(
numpy.clip(joints.position, -numpy.pi / 2, numpy.pi / 2))
return joints
def limitHeadJoints51to54(self, position):
'''
Clip joint51, joint52, joint53, joint54 to collision safe diapazone.
:param position: array of joint positions in natural order (joint51,.. joint54)
:return: clipped array.
'''
return list(
numpy.clip(position, [-1.00, -1.57, -0.55, -0.65],
[0.25, 1.57, 1.0, 0.65]))
