#include <armarx/control/common/control_law/CollisionAvoidance.h>

Classes
struct	JointRangeBufferZoneData

class	RtStatusForSafetyStrategy
	internal status of the controller, containing intermediate variables, mutable targets More...

struct	SelfCollisionData

Public Types
using	CollisionRobotIndices = std::unordered_map< unsigned int, const simox::control::robot::NodeInterface * >

using	Config = common::control_law::arondto::CollisionAvoidanceConfig

using	DistanceResult = ::simox::control::environment::DistanceResult

using	DistanceResults = std::vector< DistanceResult >

using	DynamicsModel = simox::control::dynamics::RBDLModel

using	FTConfig = common::ft::arondto::FTConfig

using	SCRobot = ::simox::control::robot::RobotInterface

Public Member Functions
	CollisionAvoidanceController (const simox::control::robot::NodeSetInterface *nodeSet)

const simox::control::robot::NodeSetInterface &	getNodeSet () const

std::size_t	numNodes () const

void	run (const Config &c, RtStatusForSafetyStrategy &robotStatus, const DistanceResults &collisionPairs, const CollisionRobotIndices &collisionRobotIndices, DynamicsModel &dynamicsModel, const Eigen::VectorXf &qpos, const Eigen::VectorXf &qvelFiltered, float torqueLimit)
	------------------------------— main rt-loop ---------------------------------------— More...

	~CollisionAvoidanceController ()

Detailed Description

Definition at line 52 of file CollisionAvoidance.h.

Member Typedef Documentation

◆ CollisionRobotIndices

using CollisionRobotIndices = std::unordered_map<unsigned int, const simox::control::robot::NodeInterface*>

Definition at line 61 of file CollisionAvoidance.h.

◆ Config

using Config = common::control_law::arondto::CollisionAvoidanceConfig

Definition at line 60 of file CollisionAvoidance.h.

◆ DistanceResult

using DistanceResult = ::simox::control::environment::DistanceResult

Definition at line 56 of file CollisionAvoidance.h.

◆ DistanceResults

using DistanceResults = std::vector<DistanceResult>

Definition at line 57 of file CollisionAvoidance.h.

◆ DynamicsModel

using DynamicsModel = simox::control::dynamics::RBDLModel

Definition at line 58 of file CollisionAvoidance.h.

◆ FTConfig

using FTConfig = common::ft::arondto::FTConfig

Definition at line 59 of file CollisionAvoidance.h.

◆ SCRobot

using SCRobot = ::simox::control::robot::RobotInterface

Definition at line 55 of file CollisionAvoidance.h.

Constructor & Destructor Documentation

◆ CollisionAvoidanceController()

CollisionAvoidanceController ( const simox::control::robot::NodeSetInterface * nodeSet )

Definition at line 21 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

◆ ~CollisionAvoidanceController()

~CollisionAvoidanceController ( )

Definition at line 30 of file CollisionAvoidance.cpp.

Member Function Documentation

◆ getNodeSet()

const simox::control::robot::NodeSetInterface& getNodeSet ( ) const

inline

Definition at line 257 of file CollisionAvoidance.h.

◆ numNodes()

std::size_t numNodes ( ) const

inline

Definition at line 251 of file CollisionAvoidance.h.

Here is the caller graph for this function:

◆ run()

void run	(	const Config &	c,
		RtStatusForSafetyStrategy &	robotStatus,
		const DistanceResults &	collisionPairs,
		const CollisionRobotIndices &	collisionRobotIndices,
		DynamicsModel &	dynamicsModel,
		const Eigen::VectorXf &	qpos,
		const Eigen::VectorXf &	qvelFiltered,
		float	torqueLimit
	)

------------------------------— main rt-loop ---------------------------------------—

--------------------------— inertia calculations -----------------------------------------------—

--------------------------— safety constraints -----------------------------------------------—

------------------------— apply EMA low pass filter --------------------------------------------—

filter safety constraint values using EMA low pass filter

assign filtered values

--------------------------— hierarchical control -----------------------------------------------— The following control hierarchies are considered:

three tasks in hierarchy: "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": true

flag: "samePriority": true same priority: torque_coll + torque_jl + torque_imp

flag: "topPrioritySelfCollisionAvoidance": true, "samePriority": false self-coll avoidance on top: torque_coll -> torque_jl -> torque_imp

flag: "topPriorityJointLimitAvoidance": true, "samePriority": false joint limit avoidance on top: torque_jl -> torque_coll -> torque_imp

flag: "topPrioritySelfCollisionAvoidance": false, "topPriorityJointLimitAvoidance": false, "samePriority": false (torque_coll + torque_jl) -> torque_imp self-coll and joint limit avoidance on same priority level:

two tasks in hierarchy:

"enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": false self-coll avoidance on top: torque_coll -> torque_imp

"enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": true joint limit avoidance on top: torque_jl -> torque_imp

one task in hierarchy (impedance control):

"enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": false only impedance control: torque_imp

three tasks in hierarchy considered

impedance control and self-collision avoidance

impedance control and joint limit avoidance

plain impedance control

----------------------— write impedance forces to buffer --------------------------------------—

--------------------------— write torque target -----------------------------------------------—

Definition at line 498 of file CollisionAvoidance.cpp.