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

Inheritance diagram for ObjectCollisionAvoidanceVelController:

Public Types
using	Config = common::control_law::arondto::ObjectCollisionAvoidanceVelConfig

Public Types inherited from CollisionAvoidanceVelController
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
void	calculateExternalCollisionVel (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

	ObjectCollisionAvoidanceVelController (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, const Eigen::VectorXf &qpos, const Eigen::VectorXf &qvelFiltered, float velocityLimit, double deltaT)
	------------------------------— main rt-loop ---------------------------------------—

Public Member Functions inherited from CollisionAvoidanceVelController
	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 ()

Additional Inherited Members
Protected Member Functions inherited from CollisionAvoidanceVelController
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 inherited from CollisionAvoidanceVelController
const Eigen::MatrixXf	I

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

RecoveryState	selfCollRecoveryState

Detailed Description

Definition at line 11 of file ObjectCollisionAvoidanceVel.h.

Member Typedef Documentation

◆ Config

using Config = common::control_law::arondto::ObjectCollisionAvoidanceVelConfig

Definition at line 16 of file ObjectCollisionAvoidanceVel.h.

Constructor & Destructor Documentation

◆ ObjectCollisionAvoidanceVelController()

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

Definition at line 11 of file ObjectCollisionAvoidanceVel.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ calculateExternalCollisionVel()

void calculateExternalCollisionVel	(	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 103 of file ObjectCollisionAvoidanceVel.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 ObjectCollisionAvoidanceVel.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,
		const Eigen::VectorXf &	qpos,
		const Eigen::VectorXf &	qvelFiltered,
		float	velocityLimit,
		double	deltaT )

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

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 high priority value to 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.

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

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

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

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Config

Constructor & Destructor Documentation

◆ ObjectCollisionAvoidanceVelController()

Member Function Documentation

◆ calculateExternalCollisionVel()

◆ calculateObjectCollisionNullspace()

◆ run()