#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

Detailed Description

Definition at line 47 of file CollisionAvoidance.h.

Member Typedef Documentation

◆ CollisionRobotIndices

using CollisionRobotIndices

Initial value:

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

Definition at line 56 of file CollisionAvoidance.h.

◆ Config

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

Definition at line 55 of file CollisionAvoidance.h.

◆ DistanceResult

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

Definition at line 51 of file CollisionAvoidance.h.

◆ DistanceResults

using DistanceResults = std::vector<DistanceResult>

Definition at line 52 of file CollisionAvoidance.h.

◆ DynamicsModel

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

Definition at line 53 of file CollisionAvoidance.h.

◆ FTConfig

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

Definition at line 54 of file CollisionAvoidance.h.

◆ SCRobot

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

Definition at line 50 of file CollisionAvoidance.h.

Constructor & Destructor Documentation

◆ CollisionAvoidanceController()

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

Definition at line 95 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ~CollisionAvoidanceController()

~CollisionAvoidanceController ( )

Definition at line 105 of file CollisionAvoidance.cpp.

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

Definition at line 24 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

Definition at line 480 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

Definition at line 265 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

Definition at line 388 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

Definition at line 114 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getIdentityMat()

const Eigen::MatrixXf & getIdentityMat ( ) const

inlineprotected

Definition at line 325 of file CollisionAvoidance.h.

Here is the caller graph for this function:

◆ getNodeSet()

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

inline

Definition at line 314 of file CollisionAvoidance.h.

◆ numNodes()

std::size_t numNodes ( ) const

inline

Definition at line 308 of file CollisionAvoidance.h.

Here is the caller graph for this function:

◆ 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 -----------------------------------------------—

Definition at line 590 of file CollisionAvoidance.cpp.

Here is the call graph for this function:

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

Detailed Description

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