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

Inheritance diagram for CollisionAvoidanceVelController:

Classes
struct	CollisionData

struct	JointRangeBufferZoneData

class	RecoveryState

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

Public Types
using	CollisionRobotIndices

using	Config = common::control_law::arondto::CollisionAvoidanceVelConfig

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
	CollisionAvoidanceVelController (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 velocityLimit, double deltaT)
	------------------------------— main rt-loop ---------------------------------------—

	~CollisionAvoidanceVelController ()

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

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

void	calculateJointLimitVel (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	calculateSelfCollisionVel (const Config &c, RtStatusForSafetyStrategy &rtStatus, RecoveryState &rState, const DistanceResults &collisionPairs, const CollisionRobotIndices &collisionRobotIndices, const Eigen::VectorXf &qvelFiltered, double deltaT) const
	avoidance torque methods

Protected Attributes
const Eigen::MatrixXf	I

const simox::control::robot::NodeSetInterface *	nodeSet

RecoveryState	selfCollRecoveryState

Detailed Description

Definition at line 47 of file CollisionAvoidanceVel.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 CollisionAvoidanceVel.h.

◆ Config

using Config = common::control_law::arondto::CollisionAvoidanceVelConfig

Definition at line 55 of file CollisionAvoidanceVel.h.

◆ DistanceResult

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

Definition at line 51 of file CollisionAvoidanceVel.h.

◆ DistanceResults

using DistanceResults = std::vector<DistanceResult>

Definition at line 52 of file CollisionAvoidanceVel.h.

◆ DynamicsModel

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

Definition at line 53 of file CollisionAvoidanceVel.h.

◆ FTConfig

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

Definition at line 54 of file CollisionAvoidanceVel.h.

◆ SCRobot

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

Definition at line 50 of file CollisionAvoidanceVel.h.

Constructor & Destructor Documentation

◆ CollisionAvoidanceVelController()

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

Definition at line 96 of file CollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ~CollisionAvoidanceVelController()

~CollisionAvoidanceVelController ( )

Definition at line 106 of file CollisionAvoidanceVel.cpp.

Member Function Documentation

◆ calculateCollisionJointVel()

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

protected

calculate common collision pair values

Definition at line 22 of file CollisionAvoidanceVel.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 426 of file CollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculateJointLimitVel()

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

protected

method:

computes the joint torques to avoid joint limits -> jointLimitJointVel

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

Definition at line 264 of file CollisionAvoidanceVel.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 337 of file CollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculateSelfCollisionVel()

void calculateSelfCollisionVel	(	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 115 of file CollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getNodeSet()

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

inline

Definition at line 300 of file CollisionAvoidanceVel.h.

◆ numNodes()

std::size_t numNodes ( ) const

inline

Definition at line 294 of file CollisionAvoidanceVel.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	velocityLimit,
		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 517 of file CollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ I

const Eigen::MatrixXf I

protected

Definition at line 307 of file CollisionAvoidanceVel.h.

◆ nodeSet

const simox::control::robot::NodeSetInterface* nodeSet

protected

Definition at line 306 of file CollisionAvoidanceVel.h.

◆ selfCollRecoveryState

RecoveryState selfCollRecoveryState

protected

Definition at line 308 of file CollisionAvoidanceVel.h.

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

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

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ CollisionRobotIndices

◆ Config

◆ DistanceResult

◆ DistanceResults

◆ DynamicsModel

◆ FTConfig

◆ SCRobot

Constructor & Destructor Documentation

◆ CollisionAvoidanceVelController()

◆ ~CollisionAvoidanceVelController()

Member Function Documentation

◆ calculateCollisionJointVel()

◆ calculateJointLimitNullspace()

◆ calculateJointLimitVel()

◆ calculateSelfCollisionNullspace()

◆ calculateSelfCollisionVel()

◆ getNodeSet()

◆ numNodes()

◆ run()

Member Data Documentation

◆ I

◆ nodeSet

◆ selfCollRecoveryState