The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. Declare the Three Odometry class The component computes the robot pose based on a complete motion model. Robot joints positions are needed to compute the forward kinematics of robot chains. Angular and robot joints rates are needed to compute the movement. More...

#include <Task.hpp>

Inheritance diagram for threed_odometry::Task:

Public Member Functions
	Task (std::string const &name="exoter_odometry::Task")

	Task (std::string const &name, RTT::ExecutionEngine *engine)

	~Task ()

bool	configureHook ()
	The following lines are template definitions for the various state machine. More...

bool	startHook ()

void	updateHook ()

void	errorHook ()

void	stopHook ()

void	cleanupHook ()

void	motionVelocities ()
	Computes the velocities using the motion model. More...

void	deadReckoning (const double &delta_t)
	Performs the odometry update. More...

void	joints_samplesUnpack (const ::base::samples::Joints &original_joints, const std::vector< std::string > &order_names, Eigen::Matrix< double, Eigen::Dynamic, 1 > &joint_positions, Eigen::Matrix< double, Eigen::Dynamic, 1 > &joint_velocities)

bool	joints_samplesMotionModel (std::vector< std::string > &order_names, const std::vector< std::string > &joint_names, const std::vector< std::string > &slip_names, const std::vector< std::string > &contact_names)

void	outputPortPose (const Eigen::Matrix< double, 6, 1 > &cartesian_velocities)
	Store the variables in the Output ports. More...

void	outputPortContactPoints ()
	Contact point information. More...

Eigen::Vector3d	boxminus (const double &w, const Eigen::Vector3d &vec, const double &scale, bool plus_minus_periodicity)

Static Public Member Functions
template<typename _MatrixType >
static _MatrixType	guaranteeSPD (const _MatrixType &A)

Protected Member Functions
virtual void	joints_samplesTransformerCallback (const base::Time &ts, const ::base::samples::Joints &joints_samples_sample)

virtual void	orientation_samplesTransformerCallback (const base::Time &ts, const ::base::samples::RigidBodyState &orientation_samples_sample)

Protected Attributes
std::string	urdfFile

std::vector< std::string >	contact_point_segments

std::vector< std::string >	contact_angle_segments

std::vector< std::string >	all_joint_names

std::vector< std::string >	slip_joint_names

std::vector< std::string >	contact_joint_names

ModelType	kinematic_model_type

IIRCoefficients	iirConfig

int	number_robot_joints

std::vector< std::string >	motion_model_joint_names

Eigen::Matrix< double, Eigen::Dynamic, 1 >	joint_positions

Eigen::Matrix< double, Eigen::Dynamic, 1 >	joint_velocities

Eigen::Matrix< double, 6, 1 >	cartesian_velocities

std::vector< Eigen::Matrix< double, 6, 1 >, Eigen::aligned_allocator< Eigen::Matrix< double, 6, 1 > > >	vector_cartesian_velocities

boost::shared_ptr< threed_odometry::KinematicKDL >	robotKinematics

boost::shared_ptr< threed_odometry::MotionModel< double > >	motionModel

std::vector< Eigen::Affine3d >	fkRobotTrans

std::vector< base::Matrix6d >	fkRobotCov

Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >	modelVelCov

Eigen::Matrix< double, 6, 6 >	cartesianVelCov

boost::shared_ptr< threed_odometry::IIR< NORDER_BESSEL_FILTER, 3 > >	bessel

WeightingMatrix	weight_matrix

::base::samples::RigidBodyState	delta_pose

::base::samples::Joints	joints_samples

::base::samples::RigidBodyState	orientation_samples

Eigen::Affine3d	pose

Eigen::Matrix< double, 6, 6 >	poseCov

Friends
class	TaskBase

Detailed Description

The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. Declare the Three Odometry class The component computes the robot pose based on a complete motion model. Robot joints positions are needed to compute the forward kinematics of robot chains. Angular and robot joints rates are needed to compute the movement.

Order of the IIR Bessel filter

The corresponding C++ class can be edited in tasks/Task.hpp and tasks/Task.cpp, and will be put in the threed_odometry namespace.

The name of a TaskContext is primarily defined via:

 deployment 'deployment_name'
     task('custom_task_name','threed_odometry::Task')
 end

It can be dynamically adapted when the deployment is called with a prefix argument.

Constructor & Destructor Documentation

Task::Task ( std::string const & name = "exoter_odometry::Task" )

TaskContext constructor for Task

Parameters

name	Name of the task. This name needs to be unique to make it identifiable via nameservices.
initial_state	The initial TaskState of the TaskContext. Default is Stopped state.

Default size for the std_vector for the Cartesian 6DoF velocities variables

Input port variables

Output port variables

Task::Task	(	std::string const &	name,
		RTT::ExecutionEngine *	engine
	)

TaskContext constructor for Task

Parameters

name	Name of the task. This name needs to be unique to make it identifiable for nameservices.
engine	The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task.

Task::~Task ( )

Default deconstructor of Task

Member Function Documentation

Eigen::Vector3d threed_odometry::Task::boxminus	(	const double &	w,
		const Eigen::Vector3d &	vec,
		const double &	scale,
		bool	plus_minus_periodicity
	)

inline

boxminus operation on a manifold

Parameters

result	`vectview` to the result
w	scalar part of input
vec	vector part of input
scale	scale result by this value
plus_minus_periodicity	if true values and give the same result

void Task::cleanupHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to PreOperational, requiring the call to configureHook() before calling start() again.

bool Task::configureHook ( )

The following lines are template definitions for the various state machine.

This hook is called by Orocos when the state machine transitions from PreOperational to Stopped. If it returns false, then the component will stay in PreOperational. Otherwise, it goes into Stopped.

It is meaningful only if the #needs_configuration has been specified in the task context definition with (for example):

task_context "TaskName" do
  needs_configuration
  ...
end

Read configuration

Configure the Motion Model of the Robot

Robot Kinematics Model

HERE YOU NEED TO PROVIDE YOUR NUMERICAL "AD HOC" KINEMATICS MODEL

Create the Motion Model

Weighting Matrix Initialization. Default is equally distributed among all the contact_points_segments

Motion Model Joints

IIR Filter

Create the Bessel Low-pass filter with the right coefficients

Inertial Noise Covariance

Resize the Velocity covariance

Angular velocity coming from gyros

Input port variables

Resize variables for Odometry

Reset initial Starting position

Set the initial world to navigation frame transform

Staring zero position

Assume well known starting position

Orientation with respect to the relative navigation frame

Assume very well known initial attitude

Get the Initial pose and uncertainty

Delta pose initialization

void Task::deadReckoning ( const double & delta_t )

Performs the odometry update.

Complete the delta pose (assuming constant acceleration)

Take uncertainty on delta orientation from the motion model

Perform the velocities integration to get the pose (Dead Reckoning)

The uncertainty needs to be transformed to the navigation frame

Dead Reckon: Propagate Pose

Adding method of propagating uncertainty

Guarantee SPD covariance

void Task::errorHook ( )

This hook is called by Orocos when the component is in the RunTimeError state, at each activity step. See the discussion in updateHook() about triggering options.

Call recover() to go back in the Runtime state.

template<typename _MatrixType >

static _MatrixType threed_odometry::Task::guaranteeSPD ( const _MatrixType & A )

inlinestatic

Single Value Decomposition

eigenvalues

bool Task::joints_samplesMotionModel	(	std::vector< std::string > &	order_names,
		const std::vector< std::string > &	joint_names,
		const std::vector< std::string > &	slip_names,
		const std::vector< std::string > &	contact_names
	)

Clean order joints names

Get only the robot physical joints

Check in case the joint is not a slip joint

Check in case the joint is not a contact angle joint

Concatenate the slip joints at the end

Concatenate the contact joints at the end

Check order names joints size

void Task::joints_samplesTransformerCallback	(	const base::Time &	ts,
		const ::base::samples::Joints &	joints_samples_sample
	)

protectedvirtual

Two different manners to get the delta time

Get the Joints values

Mechanical joints ordered by jointsName

Perform the Motion Model and get the velocities

void Task::joints_samplesUnpack	(	const ::base::samples::Joints &	original_joints,
		const std::vector< std::string > &	order_names,
		Eigen::Matrix< double, Eigen::Dynamic, 1 > &	joint_positions,
		Eigen::Matrix< double, Eigen::Dynamic, 1 > &	joint_velocities
	)

Avoid NaN values in position

Avoid NaN values in velocity

void Task::motionVelocities ( )

Computes the velocities using the motion model.

Copy Eigen to vector

Update the Motion Model (Forward Kinematics and to set Contact Points)

Compute Robot Jacobian matrix

In case of no noise in properties, set to zero to get the noise from the Motion Model

Reorganize the Jacobian matrix as required by the motion model

Get joints position and velocity ordered by Motion Model joint names

In case the information about contact angles is not NaN

Read new Weighting matrix diagonal from the Input port

The vector size should be equal to the matrix number of columns

NaN linear velocities, these are the quatities to compute by the motion model

Solve the navigation kinematics

Bessel IIR Low-pass filter of the linear cartesian_velocities from the Motion Model

Store the filtered velocity uncertainty (Uncertainty propagation is time-correlated by the IIR)

Update the Cartesian velocities on the std_vector

void Task::orientation_samplesTransformerCallback	(	const base::Time &	ts,
		const ::base::samples::RigidBodyState &	orientation_samples_sample
	)

protectedvirtual

Transformer transformation

Rotation part of the transformation in quaternion form

Two different manners to get the delta time

Get the transformation (transformation) Tbody_imu

Quaternion from Body to imu (transforming samples from imu to body)

Transform the orientation world_imu to world_body

Reset delta pose

Delta quaternion: (rotation k-1 - rotation k)

(T0_k-1)^-1 * T0_k

Angular velocity

Fill the Cartesian Velocities

Angular velocities come from gyros

Fill the Cartesian velocity covariance

Get the orientation readings

Compute Odometry only if there were samples from the joints

Perform the Dead-Reckoning

Out port the information

void Task::outputPortContactPoints ( )

Contact point information.

Debug information

Forward kinematics information. Set of contact points.

For the movement of the points with respect to the body center

void Task::outputPortPose ( const Eigen::Matrix< double, 6, 1 > & cartesian_velocities )

Store the variables in the Output ports.

Port out the OutPorts information

The Motion Model Estimated pose

NOTE: Position and orientation values are wrt the local navigation frame (frame where the dead-reckoning process "3D-Odometry" started)

timestamp;

NOTE: Linear and angular velocities are wrt the local robot body frame

NOTE: Linear and angular velocities are wrt the local navigation frame (frame where the dead-reckoning process "3D-Odometry" started)

The Delta pose of this step. Delta pose transformation with instantaneous velocity

NOTE: Linear and Angular velocities are wrt the local robot body frame

bool Task::startHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to Running. If it returns false, then the component will stay in Stopped. Otherwise, it goes into Running and updateHook() will be called.

void Task::stopHook ( )

This hook is called by Orocos when the state machine transitions from Running to Stopped after stop() has been called.

void Task::updateHook ( )

This hook is called by Orocos when the component is in the Running state, at each activity step. Here, the activity gives the "ticks" when the hook should be called.

The error(), exception() and fatal() calls, when called in this hook, allow to get into the associated RunTimeError, Exception and FatalError states.

In the first case, updateHook() is still called, and recover() allows you to go back into the Running state. In the second case, the errorHook() will be called instead of updateHook(). In Exception, the component is stopped and recover() needs to be called before starting it again. Finally, FatalError cannot be recovered.

Friends And Related Function Documentation

friend class TaskBase

friend

Member Data Documentation

std::vector<std::string> threed_odometry::Task::all_joint_names

protected

Order of Joints by Name

boost::shared_ptr< threed_odometry::IIR<NORDER_BESSEL_FILTER, 3> > threed_odometry::Task::bessel

protected

Bessel Low-pass IIR filter for the Motion Model velocities Specification of the Order and Data dimension is required

Eigen::Matrix< double, 6, 1 > threed_odometry::Task::cartesian_velocities

protected

Linear and Angular velocities NOTE: The order of the storage needs to be coincident if used as input for the motionModel

Eigen::Matrix< double, 6, 6 > threed_odometry::Task::cartesianVelCov

protected

Covariance Linear and Angular velocities NOTE: The order of the storage needs to be coincident to be used as input for the motionModel

std::vector<std::string> threed_odometry::Task::contact_angle_segments

protected

std::vector<std::string> threed_odometry::Task::contact_joint_names

protected

std::vector<std::string> threed_odometry::Task::contact_point_segments

protected

::base::samples::RigidBodyState threed_odometry::Task::delta_pose

protected

Delta pose step

std::vector<base::Matrix6d> threed_odometry::Task::fkRobotCov

protected

std::vector<Eigen::Affine3d> threed_odometry::Task::fkRobotTrans

protected

Uncertainty of the forward kinematics (if any)

IIRCoefficients threed_odometry::Task::iirConfig

protected

IIR filter configuration structure

Eigen::Matrix< double, Eigen::Dynamic, 1 > threed_odometry::Task::joint_positions

protected

Joint, Slip and Contact Angle positions NOTE: The order of the storage needs to be coincident if used as input for the motionModel

Eigen::Matrix< double, Eigen::Dynamic, 1 > threed_odometry::Task::joint_velocities

protected

Joint, Slip and Contact Angle velocities NOTE: The order of the storage needs to be coincident if used as input for the motionModel

::base::samples::Joints threed_odometry::Task::joints_samples

protected

Input port variables

ModelType threed_odometry::Task::kinematic_model_type

protected

Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > threed_odometry::Task::modelVelCov

protected

Covariance Joints, Slip and Contact Angle velocities NOTE: The order of the storage needs to be coincident if used as input for the motionModel

std::vector<std::string> threed_odometry::Task::motion_model_joint_names

protected

Name of all joints model names

boost::shared_ptr< threed_odometry::MotionModel<double> > threed_odometry::Task::motionModel

protected

Robot Motion Model Forward kinematics of the robot chains

int threed_odometry::Task::number_robot_joints

protected

Number of physical joints according to the model and task properties

::base::samples::RigidBodyState threed_odometry::Task::orientation_samples

protected

Eigen::Affine3d threed_odometry::Task::pose

protected

Output port variables Body Center w.r.t the World Coordinate system (using statistical Motion Model and IMU orientation)

Eigen::Matrix<double, 6, 6> threed_odometry::Task::poseCov

protected

boost::shared_ptr< threed_odometry::KinematicKDL > threed_odometry::Task::robotKinematics

protected

Robot Kinematic Model

std::vector<std::string> threed_odometry::Task::slip_joint_names

protected

std::string threed_odometry::Task::urdfFile

protected

std::vector< Eigen::Matrix <double, 6, 1> , Eigen::aligned_allocator < Eigen::Matrix <double, 6, 1> > > threed_odometry::Task::vector_cartesian_velocities

protected

Buffer for the storage of cartesian_velocities variables (for integration assuming constant acceleration)

WeightingMatrix threed_odometry::Task::weight_matrix

protected

Weighting Matrix used in the Motion Model

The documentation for this class was generated from the following files:

/build/rock-master-18.09-slam-orogen-threed-odometry-0.20160923/tasks/Task.hpp
/build/rock-master-18.09-slam-orogen-threed-odometry-0.20160923/tasks/Task.cpp

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation