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]))