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

Inheritance diagram for CollisionAvoidanceController:

Classes
struct	AdmittanceInterface

struct	CollisionData

struct	JointRangeBufferZoneData

struct	RecoveryState

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

Public Types
using	CollisionRobotIndices

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::simox::robot::Robot

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, RecoveryState &rStateSelfColl, const DistanceResults &collisionPairs, const CollisionRobotIndices &collisionRobotIndices, DynamicsModel &dynamicsModel, const Eigen::VectorXf &qpos, const Eigen::VectorXf &qvelFiltered, float torqueLimit, double deltaT)
	------------------------------— main rt-loop ---------------------------------------—

	~CollisionAvoidanceController ()

Protected Member Functions
void	calculateCollisionTorque (CollisionData &collDataVec, const Config &c, RtStatusForSafetyStrategy &rts, const CollisionRobotIndices &collisionRobotIndices, const Eigen::VectorXf &qvelFiltered, Eigen::VectorXf &jointTorque, float distanceThresholdRaw, float distanceThreshold) const

void	calculateJointLimitNullspace (const Config &c, RtStatusForSafetyStrategy &rtStatus, const Eigen::VectorXf &qpos) const

void	calculateJointLimitTorque (const Config &c, RtStatusForSafetyStrategy &rtStatus, const Eigen::VectorXf &qpos, const Eigen::VectorXf &qvelFiltered) const

void	calculateSelfCollisionNullspace (const Config &c, RtStatusForSafetyStrategy &rtStatus) const
	caclulate null spaces

void	calculateSelfCollisionTorque (const Config &c, RtStatusForSafetyStrategy &rtStatus, RecoveryState &rState, const DistanceResults &collisionPairs, const CollisionRobotIndices &collisionRobotIndices, const Eigen::VectorXf &qvelFiltered, double deltaT) const
	avoidance torque methods

const Eigen::MatrixXf &	getIdentityMat () const

Member Typedef Documentation

◆ CollisionRobotIndices

using CollisionRobotIndices

Initial value:

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

◆ Config

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

◆ DistanceResult

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

◆ DistanceResults

using DistanceResults = std::vector<DistanceResult>

◆ DynamicsModel

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

◆ FTConfig

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

◆ SCRobot

using SCRobot = simox::control::simox::robot::Robot

Constructor & Destructor Documentation

◆ CollisionAvoidanceController()

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

◆ ~CollisionAvoidanceController()

~CollisionAvoidanceController ( )

Member Function Documentation

◆ calculateCollisionTorque()

void calculateCollisionTorque	(	CollisionData &	collDataVec,
		const Config &	c,
		RtStatusForSafetyStrategy &	rts,
		const CollisionRobotIndices &	collisionRobotIndices,
		const Eigen::VectorXf &	qvelFiltered,
		Eigen::VectorXf &	jointTorque,
		float	distanceThresholdRaw,
		float	distanceThreshold ) const

protected

calculate common collision pair values

◆ calculateJointLimitNullspace()

void calculateJointLimitNullspace	(	const Config &	c,
		RtStatusForSafetyStrategy &	rtStatus,
		const Eigen::VectorXf &	qpos ) const

protected

check fundamentals thesis BA Jan Fenker

get desired null space value based on proximity to joint boundaries

full locking

in transition state

full locking

unrestricted movement

set desired null space value in null space matrix

◆ calculateJointLimitTorque()

void calculateJointLimitTorque	(	const Config &	c,
		RtStatusForSafetyStrategy &	rtStatus,
		const Eigen::VectorXf &	qpos,
		const Eigen::VectorXf &	qvelFiltered ) const

protected

method:

computes the joint torques to avoid joint limits -> jointLimitJointTorque

iterate over all joints and check the joint positions for not limiteless joints

◆ calculateSelfCollisionNullspace()

void calculateSelfCollisionNullspace	(	const Config &	c,
		RtStatusForSafetyStrategy &	rtStatus ) const

protected

caclulate null spaces

get desired null space value based on proximity to collision

full locking in direction of collision

in transition state

unrestricted movement in direction of collision

normalize projected Jacobian

check, whether impedance joint torque acts against the collision direction

project desired null space in corresponding direction via the Norm-Jacobian

◆ calculateSelfCollisionTorque()

void calculateSelfCollisionTorque	(	const Config &	c,
		RtStatusForSafetyStrategy &	rtStatus,
		RecoveryState &	rState,
		const DistanceResults &	collisionPairs,
		const CollisionRobotIndices &	collisionRobotIndices,
		const Eigen::VectorXf &	qvelFiltered,
		double	deltaT ) const

protected

avoidance torque methods

methods for self-collision and joint limit avoidance avoidance torque methods

self-collision avoidance algorithm following the methods in Dietrich et al. (2012):

A. Dietrich, T. Wimbock, A. Albu-Schaffer and G. Hirzinger, "Integration of Reactive, Torque-Based Self-Collision Avoidance Into a Task Hierarchy," in IEEE Transactions on Robotics, vol. 28, no. 6, pp. 1278-1293, Dec. 2012, doi: 10.1109/TRO.2012.2208667.

see: https://ieeexplore.ieee.org/document/6255795

method:

computes the joint torques to avoid self-collisions -> collisionJointTorque

clear values before new cycle

when collDistanceThresholdInit is smaller than recoveryDistanceElapseMeter (usually means it is zero), we need to initilize it to the smallest collision distance plus a recoveryDistanceElapseMeter.

check collision pairs, which have are below the prefilter distance

if the points are the same, normalVec comes with a value

◆ getIdentityMat()

const Eigen::MatrixXf & getIdentityMat ( ) const

inlineprotected

◆ getNodeSet()

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

inline

◆ numNodes()

std::size_t numNodes ( ) const

inline

◆ run()

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

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

run in rt thread --------------------------— 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 -----------------------------------------------—

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

armarx/control/common/control_law/CollisionAvoidance.h
armarx/control/common/control_law/CollisionAvoidance.cpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Member Typedef Documentation

◆ CollisionRobotIndices

◆ Config

◆ DistanceResult

◆ DistanceResults

◆ DynamicsModel

◆ FTConfig

◆ SCRobot

Constructor & Destructor Documentation

◆ CollisionAvoidanceController()

◆ ~CollisionAvoidanceController()

Member Function Documentation

◆ calculateCollisionTorque()

◆ calculateJointLimitNullspace()

◆ calculateJointLimitTorque()

◆ calculateSelfCollisionNullspace()

◆ calculateSelfCollisionTorque()

◆ getIdentityMat()

◆ getNodeSet()

◆ numNodes()

◆ run()