SafetyTaskspaceImpedanceController Class Reference

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

Classes
struct	RtStatus
	internal status of the controller, containing intermediate variables, mutable targets More...

Public Types
using	Config = common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfig
using	ConfigDict = common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfigDict
using	DistanceResultsPtr = std::shared_ptr< std::vector< simox::control::environment::DistanceResult > >
using	FTConfig = common::ft::arondto::FTConfig
using	InertiaPtr = std::shared_ptr< simox::control::geodesics::metric::Inertia >
using	SimoxRobotPtr = std::shared_ptr< simox::control::simox::robot::Robot >

Public Member Functions
void	firstRun ()
void	initialize (const VirtualRobot::RobotNodeSetPtr &rns, const VirtualRobot::RobotNodeSetPtr &rtRns, SimoxRobotPtr &simoxControlRobot)
void	preactivateInit (const VirtualRobot::RobotNodeSetPtr &rns, Config &c, RtStatus &rtStatus)
void	run (Config &c, RtStatus &robotStatus, DistanceResultsPtr &collisionPairs, std::shared_ptr< std::vector< int >> &pointsOnArm, int pointsOnArmIndex, InertiaPtr &inertiaInstance, std::vector< std::string > &selfCollisionNodes)
	------------------------------— main rt-loop ---------------------------------------— More...

Public Attributes
common::ft::FTSensor	ftsensor
unsigned int	numOfJoints
VirtualRobot::RobotNodePtr	rtTCP
VirtualRobot::RobotNodePtr	tcp

Detailed Description

Definition at line 45 of file SafetyTaskspaceImpedanceController.h.

Member Typedef Documentation

Config

using Config = common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfig

Definition at line 53 of file SafetyTaskspaceImpedanceController.h.

ConfigDict

using ConfigDict = common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfigDict

Definition at line 55 of file SafetyTaskspaceImpedanceController.h.

DistanceResultsPtr

using DistanceResultsPtr = std::shared_ptr<std::vector<simox::control::environment::DistanceResult> >

Definition at line 50 of file SafetyTaskspaceImpedanceController.h.

FTConfig

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

Definition at line 52 of file SafetyTaskspaceImpedanceController.h.

InertiaPtr

using InertiaPtr = std::shared_ptr<simox::control::geodesics::metric::Inertia>

Definition at line 48 of file SafetyTaskspaceImpedanceController.h.

SimoxRobotPtr

using SimoxRobotPtr = std::shared_ptr<simox::control::simox::robot::Robot>

Definition at line 51 of file SafetyTaskspaceImpedanceController.h.

Member Function Documentation

firstRun()

void firstRun

(

)

Definition at line 132 of file SafetyTaskspaceImpedanceController.cpp.

Here is the call graph for this function:

initialize()

void initialize	(	const VirtualRobot::RobotNodeSetPtr &	rns,
		const VirtualRobot::RobotNodeSetPtr &	rtRns,
		SimoxRobotPtr &	simoxControlRobot
	)

Definition at line 15 of file SafetyTaskspaceImpedanceController.cpp.

Here is the call graph for this function:

preactivateInit()

void preactivateInit	(	const VirtualRobot::RobotNodeSetPtr &	rns,
		Config &	c,
		RtStatus &	rtStatus
	)

calculate weights for self-collision nullspace transition function range between z1 and z2 z1: below this distance [m] the collision direction is fully locked z2: above this distance collision direction is unrestricted

obtain joint information (limits, limitless) from VirtualRobot

set joint limit avoidance threshold parameters for not limiteless joints

from here: only the parameter output information is prepared

Definition at line 36 of file SafetyTaskspaceImpedanceController.cpp.

run()

void run	(	Config &	c,
		RtStatus &	robotStatus,
		DistanceResultsPtr &	collisionPairs,
		std::shared_ptr< std::vector< int >> &	pointsOnArm,
		int	pointsOnArmIndex,
		InertiaPtr &	inertiaInstance,
		std::vector< std::string > &	selfCollisionNodes
	)

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

run in rt thread

this should not be moved to non rt, just in case non rt thread dies, rt still functions as safely

---------------------------— compute jacobi ----------------------------------------------------------—

---------------------------— update velocity/twist ---------------------------------------------------—

----------------------------------— Impedance control ------------------------------------------------— calculate pose error between target pose and current pose !!! This is very important: you have to keep postion and orientation both with UI unit (meter, radian) to calculate impedance force.

--------------------------— Nullspace PD Control -----------------------------------------------—

--------------------------— Map TS target force to JS -----------------------------------------------—

--------------------------— Joint Space Damping -----------------------------------------------—

--------------------------— 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 1022 of file SafetyTaskspaceImpedanceController.cpp.

Here is the call graph for this function:

Member Data Documentation

ftsensor

common::ft::FTSensor ftsensor

Definition at line 359 of file SafetyTaskspaceImpedanceController.h.

numOfJoints

unsigned int numOfJoints

Definition at line 324 of file SafetyTaskspaceImpedanceController.h.

rtTCP

VirtualRobot::RobotNodePtr rtTCP

Definition at line 358 of file SafetyTaskspaceImpedanceController.h.

tcp

VirtualRobot::RobotNodePtr tcp

Definition at line 357 of file SafetyTaskspaceImpedanceController.h.

---

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

• armarx/control/common/control_law/SafetyTaskspaceImpedanceController.h
• armarx/control/common/control_law/SafetyTaskspaceImpedanceController.cpp