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

Classes
struct	Config
	you can set the following values from outside of the rt controller via Ice interfaces More...

struct	NonRtStatus

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

Public Member Functions
void	firstRun ()

void	initialize (const VirtualRobot::RobotNodeSetPtr &rns)

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

void	run (const Config &cfg, ControlTarget &targets)

bool	updateControlStatus (const Config &cfg, const RobotStatus &robotStatus)

Public Attributes
TripleBuffer< NonRtStatus >	bufferNonRtToOnPublish

TripleBuffer< NonRtStatus >	bufferNonRtToRt

TripleBuffer< RtStatus >	bufferRtToOnPublish

std::atomic< bool >	isInitialized {false}

VirtualRobot::RobotNodePtr	tcp

Detailed Description

Definition at line 37 of file KeypointsImpedanceController.h.

Member Function Documentation

◆ firstRun()

void firstRun ( )

Definition at line 99 of file KeypointsImpedanceController.cpp.

Here is the call graph for this function:

◆ initialize()

void initialize ( const VirtualRobot::RobotNodeSetPtr & rns )

joint space variables

Definition at line 23 of file KeypointsImpedanceController.cpp.

Here is the call graph for this function:

◆ preactivateInit()

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

keypoint status

Definition at line 39 of file KeypointsImpedanceController.cpp.

Here is the call graph for this function:

◆ run()

void run	(	const Config &	cfg,
		ControlTarget &	targets
	)

run in rt thread

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

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

Definition at line 246 of file KeypointsImpedanceController.cpp.

Here is the call graph for this function:

◆ updateControlStatus()

bool updateControlStatus	(	const Config &	cfg,
		const RobotStatus &	robotStatus
	)

check size and value

--------------------------— get current status of robot ------------------------------------------— original data in ArmarX use the following units: position in mm, joint angles and orientation in radian, velocity in mm/s and radian/s, etc. here we convert mm to m, if you use MP from outside, make sure to convert it back to mm

--------------------------— keypoint related ------------------------------------------— compute (filtered) keypoint position

compute filtered keypoint velocity

--------------------------— compute keypoint tracking force ------------------------------------------—

in theory, we can also add s.desiredKeypointVelocity to the damping term

Definition at line 117 of file KeypointsImpedanceController.cpp.

Here is the call graph for this function: