ObjectCollisionAvoidanceController Class Reference

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

Inheritance diagram for ObjectCollisionAvoidanceController:

Public Types
using	Config = common::control_law::arondto::ObjectCollisionAvoidanceConfig
Public Types inherited from CollisionAvoidanceController
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
void	calculateExternalCollisionTorque (const Config &c, RtStatusForSafetyStrategy &rts, RecoveryState &rState, const DistanceResults &externalCollisionPairs, const CollisionRobotIndices &collisionRobotIndices, const Eigen::VectorXf &qvelFiltered, double deltaT) const
void	calculateObjectCollisionNullspace (const Config &c, RtStatusForSafetyStrategy &rtStatus) const
	ObjectCollisionAvoidanceController (const simox::control::robot::NodeSetInterface *nodeSet)
void	run (const Config &c, RtStatusForSafetyStrategy &robotStatus, RecoveryState &rStateSelfColl, RecoveryState &rStateObjColl, const DistanceResults &collisionPairs, const DistanceResults &externalCollisionPairs, const CollisionRobotIndices &collisionRobotIndices, DynamicsModel &dynamicsModel, float torqueLimit, float jointVelLimit, TSCtrlRtStatus &rts)
	------------------------------— main rt-loop ---------------------------------------—
Public Member Functions inherited from CollisionAvoidanceController
	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 ()

Additional Inherited Members
Protected Member Functions inherited from CollisionAvoidanceController
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 11 of file ObjectCollisionAvoidance.h.

Member Typedef Documentation

Config

using Config = common::control_law::arondto::ObjectCollisionAvoidanceConfig

Definition at line 16 of file ObjectCollisionAvoidance.h.

Constructor & Destructor Documentation

ObjectCollisionAvoidanceController()

ObjectCollisionAvoidanceController

(

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

nodeSet

)

Definition at line 11 of file ObjectCollisionAvoidance.cpp.

Here is the call graph for this function:

Member Function Documentation

calculateExternalCollisionTorque()

void calculateExternalCollisionTorque	(	const Config &	c,
		RtStatusForSafetyStrategy &	rts,
		RecoveryState &	rState,
		const DistanceResults &	externalCollisionPairs,
		const CollisionRobotIndices &	collisionRobotIndices,
		const Eigen::VectorXf &	qvelFiltered,
		double	deltaT ) const

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 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.

handling of external collision pairs

only node1 is on the robot

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

Definition at line 106 of file ObjectCollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

calculateObjectCollisionNullspace()

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

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 19 of file ObjectCollisionAvoidance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

run()

void run	(	const Config &	c,
		RtStatusForSafetyStrategy &	robotStatus,
		RecoveryState &	rStateSelfColl,
		RecoveryState &	rStateObjColl,
		const DistanceResults &	collisionPairs,
		const DistanceResults &	externalCollisionPairs,
		const CollisionRobotIndices &	collisionRobotIndices,
		DynamicsModel &	dynamicsModel,
		float	torqueLimit,
		float	jointVelLimit,
		TSCtrlRtStatus &	rts )

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

there are 4 components generating torque:

• SelfCollisionAvoidance
• ObjectCollisionAvoidance
• JointLimitAvoidance
• Impedance

For each of the enabled components a priority is loaded from the config. The impedance component is always enabled.

The components are processed in descending order from low priority (high priority value) ===> high priority (low priority value)

If two components a and b have the same priority value their torques are added and their null spaces combined: torque_combined = torque_a + torque_b null_space_combined = null_space_a * null_space_b

The torques of two component groups a and b (with a representing all components with a higher priority value than priority_b and b representing all components with a priority value of priority_b) are calculated like this: torque_combined = torque_b + (null_space_b * torque_a)

Iteratively all torques are combined this way.

If two tasks have the exact same priority, combine them

Otherwise, do the nullspace projection

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

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

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

Definition at line 261 of file ObjectCollisionAvoidance.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/ObjectCollisionAvoidance.h
• armarx/control/common/control_law/ObjectCollisionAvoidance.cpp