State observers available for Upkies. More...

Classes
class	BaseOrientation
	Observe the orientation of the base frame relative to the world frame. More...

class	FloorContact
	Observe contact between the wheels and the floor. More...

class	HistoryObserver
	Report high-frequency history vectors to lower-frequency agents. More...

class	Observer
	Base class for observers. More...

class	ObserverPipeline
	Observer pipeline. More...

class	WheelContact
	Observe contact between a given wheel and the floor. More...

class	WheelOdometry
	Observe the relative motion of the floating base with respect to the floor. More...

Functions
Eigen::Matrix3d	compute_base_orientation_from_imu (const Eigen::Quaterniond &quat_imu_in_ars, const Eigen::Matrix3d &rotation_base_to_imu, const Eigen::Matrix3d &rotation_ars_to_world)
	Get the orientation of the base frame with respect to the world frame. More...

double	compute_pitch_frame_in_parent (const Eigen::Matrix3d &orientation_frame_in_parent)
	Get pitch angle of a given frame relative to the parent vertical. More...

double	compute_base_pitch_from_imu (const Eigen::Quaterniond &quat_imu_in_ars, const Eigen::Matrix3d &rotation_base_to_imu, const Eigen::Matrix3d &rotation_ars_to_world)
	Get pitch angle of the base frame relative to the world frame. More...

Eigen::Vector3d	compute_base_angular_velocity_from_imu (const Eigen::Vector3d &angular_velocity_imu_in_imu, const Eigen::Matrix3d &rotation_base_to_imu)
	Compute the body angular velocity of the base from IMU readings. More...

void	observe_servos (palimpsest::Dictionary &observation, const std::map< int, std::string > &servo_name_map, const std::vector< ServoReply > &servo_replies)
	Observe servo measurements. More...

void	observe_time (Dictionary &observation)
	Observe time since the epoch. More...

Detailed Description

State observers available for Upkies.

State observation.

Function Documentation

◆ compute_base_angular_velocity_from_imu()

Eigen::Vector3d upkie::cpp::observers::compute_base_angular_velocity_from_imu	(	const Eigen::Vector3d &	angular_velocity_imu_in_imu,
		const Eigen::Matrix3d &	rotation_base_to_imu
	)

inline

Compute the body angular velocity of the base from IMU readings.

Parameters

[in]	angular_velocity_imu_in_imu	Angular velocity from the IMU.
[in]	rotation_base_to_imu	Rotation matrix from base to IMU.

Returns: Body angular velocity of the base frame.

Calculation checks:

\(I\): IMU frame
\(B\): base frame
\(W\): world (inertial) frame

Our input is \({}_I \omega_{WI}\), we seek \({}_B \omega_{WB}\).

\( \begin{align*} \dot{R}_{WI} & = R_{WI} ({}_I \omega_{WI} \times) \\ R_{WB} & = R_{WI} R_{IB} \\ \dot{R}_{WB} & = \dot{R}_{WI} R_{IB} \\ & = R_{WI} ({}_I \omega_{WI} \times) R_{IB} \\ & = R_{WI} R_{IB} ({}_B \omega_{WI} \times) \\ & = R_{WB} ({}_B \omega_{WI} \times) = R_{WB} ({}_B \omega_{WB} \times) \end{align*} \)

Thus \({}_B \omega_{WB} = R_{BI} {}_I \omega_{WI}\).

◆ compute_base_orientation_from_imu()

Eigen::Matrix3d upkie::cpp::observers::compute_base_orientation_from_imu	(	const Eigen::Quaterniond &	quat_imu_in_ars,
		const Eigen::Matrix3d &	rotation_base_to_imu,
		const Eigen::Matrix3d &	rotation_ars_to_world
	)

inline

Get the orientation of the base frame with respect to the world frame.

Parameters

quat_imu_in_ars	Quaternion representing the rotation matrix from the IMU frame to the attitude reference system (ARS) frame, in `[w, x, y, z]` format.
rotation_base_to_imu	Rotation matrix from the base frame to the IMU frame. When not specified, the default Upkie mounting orientation is used.
rotation_ars_to_world	Rotation from the attitude reference system (ARS) frame to the world frame.

Returns: Rotation matrix from the base frame to the world frame.

◆ compute_base_pitch_from_imu()

double upkie::cpp::observers::compute_base_pitch_from_imu	(	const Eigen::Quaterniond &	quat_imu_in_ars,
		const Eigen::Matrix3d &	rotation_base_to_imu,
		const Eigen::Matrix3d &	rotation_ars_to_world
	)

inline

Get pitch angle of the base frame relative to the world frame.

Parameters

[in]	quat_imu_in_ars	Quaternion representing the rotation matrix from the IMU frame to the attitude reference system (ARS) frame.
[in]	rotation_base_to_imu	Rotation matrix from the base frame to the IMU frame. When not specified, the default Upkie mounting orientation is used.
	rotation_ars_to_world	Rotation from the attitude reference system (ARS) frame to the world frame.

Returns: Angle from the world z-axis (unit vector opposite to gravity) to the base z-axis. This angle is positive when the base leans forward.

◆ compute_pitch_frame_in_parent()

double upkie::cpp::observers::compute_pitch_frame_in_parent ( const Eigen::Matrix3d & orientation_frame_in_parent )

inline

Get pitch angle of a given frame relative to the parent vertical.

Figure:

parent z
  ^    frame z
  |     /
  |    /
  |   /
  |  /
  | /
  |/
 (x)-----------> heading vector
   \\  )
    \\+  positive pitch
     \\
      \\
       \\
      frame x

Parameters

orientation_frame_in_parent Rotation matrix from the target frame to the parent frame.

Returns: Angle from the parent z-axis (gravity) to the frame z-axis. Equivalently, angle from the heading vector to the sagittal vector of the frame.

Note: Angle is positive in the trigonometric sense (CCW positive, CW negative) in the heading-vertical plane directed by the lateral vector, which is (x) in the above schematic (pointing away) and not (.) (pointing from screen to the reader).

◆ observe_servos()

void upkie::cpp::observers::observe_servos	(	palimpsest::Dictionary &	observation,
		const std::map< int, std::string > &	servo_name_map,
		const std::vector< ServoReply > &	servo_replies
	)

Observe servo measurements.

Parameters

[out]	observation	Dictionary to write observations to.
[in]	servo_name_map	Map from servo ID to joint name.
[in]	servo_replies	List of servo replies from the CAN bus.

Exceptions

ServoError If a servo reply is invalid.

◆ observe_time()

void upkie::cpp::observers::observe_time ( Dictionary & observation )

inline

Observe time since the epoch.

Parameters

[out] observation Dictionary to write observations to.

Classes

Functions

Detailed Description

Function Documentation

◆ compute_base_angular_velocity_from_imu()

◆ compute_base_orientation_from_imu()

◆ compute_base_pitch_from_imu()

◆ compute_pitch_frame_in_parent()

◆ observe_servos()

◆ observe_time()