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

Public Types
using	Config = ConfigType

using	ConfigDict = ConfigDictType

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

Public Member Functions
void	applyAdmittanceInterface (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	applyInverseDynamics (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	applyInverseKinematics (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	computeInertiaInverse (TSCtrlRtStatus &rtStatus)

void	firstRun ()

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

bool	isControlModeValid (const std::vector< std::string > &nameList, const std::map< std::string, std::string > &jointControlModeMap)

bool	isForceTorqueSafe ()

virtual void	run (ConfigType &c, TSCtrlRtStatus &robotStatus)=0

void	setForceTorqueGuard (bool forceGuard, bool torqueGuard)

void	updateJacobian (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	updateNullspaceForceCommand (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	updateNullspaceTwistCommand (ConfigType &c, TSCtrlRtStatus &rtStatus)
	Admittance.

void	updateTSAdmittanceCommand (ConfigType &c, TSCtrlRtStatus &rtStatus)
	Impedance.

void	updateTSError (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	updateTSForceCommand (ConfigType &c, TSCtrlRtStatus &rtStatus)
	Velocity.

void	updateTSTwistCommand (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	updateTwist (ConfigType &c, TSCtrlRtStatus &rtStatus)

void	validate (ConfigType &c, TSCtrlRtStatus &rtStatus)

virtual	~TaskspaceController ()=default

Public Attributes
common::ft::FTSensor	ftsensor

VirtualRobot::RobotNodePtr	rtTCP

VirtualRobot::RobotNodePtr	tcp

Detailed Description

template<typename ConfigType, typename ConfigDictType>
class armarx::control::common::control_law::TaskspaceController< ConfigType, ConfigDictType >

Definition at line 47 of file TaskspaceController.h.

Member Typedef Documentation

◆ Config

template<typename ConfigType, typename ConfigDictType>

using Config = ConfigType

Definition at line 51 of file TaskspaceController.h.

◆ ConfigDict

template<typename ConfigType, typename ConfigDictType>

using ConfigDict = ConfigDictType

Definition at line 52 of file TaskspaceController.h.

◆ FTConfig

template<typename ConfigType, typename ConfigDictType>

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

Definition at line 50 of file TaskspaceController.h.

Constructor & Destructor Documentation

◆ ~TaskspaceController()

template<typename ConfigType, typename ConfigDictType>

virtual ~TaskspaceController ( )

virtualdefault

Member Function Documentation

◆ applyAdmittanceInterface()

template<typename ConfigType, typename ConfigDictType>

void applyAdmittanceInterface	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Definition at line 367 of file TaskspaceController.cpp.

◆ applyInverseDynamics()

template<typename ConfigType, typename ConfigDictType>

void applyInverseDynamics	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Map TS target force to JS

write torque target

Definition at line 238 of file TaskspaceController.cpp.

◆ applyInverseKinematics()

template<typename ConfigType, typename ConfigDictType>

void applyInverseKinematics	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Map TS target velocity to JS

Definition at line 257 of file TaskspaceController.cpp.

◆ computeInertiaInverse()

template<typename ConfigType, typename ConfigDictType>

void computeInertiaInverse ( TSCtrlRtStatus & rtStatus )

Determine the size of the submatrix (M x M)

Extract elements: Loop over rows and columns

Invert and cast to float

Definition at line 333 of file TaskspaceController.cpp.

◆ firstRun()

template<typename ConfigType, typename ConfigDictType>

void firstRun ( )

Definition at line 26 of file TaskspaceController.cpp.

◆ initialize()

template<typename ConfigType, typename ConfigDictType>

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

Definition at line 95 of file TaskspaceController.h.

◆ isControlModeValid()

template<typename ConfigType, typename ConfigDictType>

bool isControlModeValid	(	const std::vector< std::string > &	nameList,
		const std::map< std::string, std::string > &	jointControlModeMap )

Definition at line 33 of file TaskspaceController.cpp.

◆ isForceTorqueSafe()

template<typename ConfigType, typename ConfigDictType>

bool isForceTorqueSafe ( )

Definition at line 394 of file TaskspaceController.cpp.

◆ run()

template<typename ConfigType, typename ConfigDictType>

virtual void run	(	ConfigType &	c,
		TSCtrlRtStatus &	robotStatus )

pure virtual

Implemented in TSAdmController, TSImpController, TSMixImpVelController, and TSVelController.

◆ setForceTorqueGuard()

template<typename ConfigType, typename ConfigDictType>

void setForceTorqueGuard	(	bool	forceGuard,
		bool	torqueGuard )

Definition at line 277 of file TaskspaceController.cpp.

◆ updateJacobian()

template<typename ConfigType, typename ConfigDictType>

void updateJacobian	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

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

Definition at line 111 of file TaskspaceController.cpp.

◆ updateNullspaceForceCommand()

template<typename ConfigType, typename ConfigDictType>

void updateNullspaceForceCommand	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Nullspace PD Control

Definition at line 202 of file TaskspaceController.cpp.

◆ updateNullspaceTwistCommand()

template<typename ConfigType, typename ConfigDictType>

void updateNullspaceTwistCommand	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Admittance.

Nullspace PD Control

Definition at line 220 of file TaskspaceController.cpp.

◆ updateTSAdmittanceCommand()

template<typename ConfigType, typename ConfigDictType>

void updateTSAdmittanceCommand	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Impedance.

-------------------------— admittance control ------------------------------------------— calculate pose error between the virtual pose and the target pose

admittance control law and Euler Integration -> virtual pose

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

Definition at line 285 of file TaskspaceController.cpp.

◆ updateTSError()

template<typename ConfigType, typename ConfigDictType>

void updateTSError	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

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

position error in meter

Definition at line 159 of file TaskspaceController.cpp.

◆ updateTSForceCommand()

template<typename ConfigType, typename ConfigDictType>

void updateTSForceCommand	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

Velocity.

Unit in N, and Nm

Definition at line 191 of file TaskspaceController.cpp.

◆ updateTSTwistCommand()

template<typename ConfigType, typename ConfigDictType>

void updateTSTwistCommand	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

------------------------— Velocity control ----------------------------------------— Unit in m/s and rad/s

Definition at line 180 of file TaskspaceController.cpp.

◆ updateTwist()

template<typename ConfigType, typename ConfigDictType>

void updateTwist	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

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

in m/s

Definition at line 143 of file TaskspaceController.cpp.

◆ validate()

template<typename ConfigType, typename ConfigDictType>

void validate	(	ConfigType &	c,
		TSCtrlRtStatus &	rtStatus )

run in rt thread

safety guard by FT and target pose

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

Definition at line 61 of file TaskspaceController.cpp.

Member Data Documentation

◆ ftsensor

template<typename ConfigType, typename ConfigDictType>

common::ft::FTSensor ftsensor

Definition at line 57 of file TaskspaceController.h.

◆ rtTCP

template<typename ConfigType, typename ConfigDictType>

VirtualRobot::RobotNodePtr rtTCP

Definition at line 56 of file TaskspaceController.h.

◆ tcp

template<typename ConfigType, typename ConfigDictType>

VirtualRobot::RobotNodePtr tcp

Definition at line 55 of file TaskspaceController.h.

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

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

Public Types

Public Member Functions

Public Attributes

Detailed Description

Member Typedef Documentation

◆ Config

◆ ConfigDict

◆ FTConfig

Constructor & Destructor Documentation

◆ ~TaskspaceController()

Member Function Documentation

◆ applyAdmittanceInterface()

◆ applyInverseDynamics()

◆ applyInverseKinematics()

◆ computeInertiaInverse()

◆ firstRun()

◆ initialize()

◆ isControlModeValid()

◆ isForceTorqueSafe()

◆ run()

◆ setForceTorqueGuard()

◆ updateJacobian()

◆ updateNullspaceForceCommand()

◆ updateNullspaceTwistCommand()

◆ updateTSAdmittanceCommand()

◆ updateTSError()

◆ updateTSForceCommand()

◆ updateTSTwistCommand()

◆ updateTwist()

◆ validate()

Member Data Documentation

◆ ftsensor

◆ rtTCP

◆ tcp