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

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

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

Public Member Functions
void	firstRun ()

void	initialize (const VirtualRobot::RobotNodeSetPtr &rns, const std::string &configFileName)

void	preactivateInit (const VirtualRobot::RobotNodeSetPtr &rns)

Config	reconfigure (const std::string &configFileName)

void	run (bool rtReady, std::vector< ControlTarget1DoFActuatorTorque * > targets)

bool	updateControlStatus (const Config &cfg, const IceUtil::Time &timeSinceLastIteration, std::vector< const SensorValue1DoFActuatorTorque * > torqueSensors, std::vector< const SensorValue1DoFActuatorVelocity * > velocitySensors, std::vector< const SensorValue1DoFActuatorPosition * > positionSensors)

bool	validateTargetPose (Eigen::Matrix4f &targetPose)

Public Attributes
Status	s

VirtualRobot::RobotNodePtr	tcp

Detailed Description

Definition at line 33 of file KeypointsAdmittanceController.h.

Member Function Documentation

◆ firstRun()

void firstRun ( )

Definition at line 194 of file KeypointsAdmittanceController.cpp.

◆ initialize()

void initialize	(	const VirtualRobot::RobotNodeSetPtr &	rns,
		const std::string &	configFileName
	)

joint space variables

parse configuration files

keypoints related

Definition at line 22 of file KeypointsAdmittanceController.cpp.

Here is the call graph for this function:

◆ preactivateInit()

void preactivateInit ( const VirtualRobot::RobotNodeSetPtr & rns )

Definition at line 152 of file KeypointsAdmittanceController.cpp.

◆ reconfigure()

KeypointsAdmittanceController::Config reconfigure ( const std::string & configFileName )

keypoint related

Definition at line 118 of file KeypointsAdmittanceController.cpp.

Here is the call graph for this function:

◆ run()

void run	(	bool	rtReady,
		std::vector< ControlTarget1DoFActuatorTorque * >	targets
	)

only when the ft sensor is calibrated can we allow the admittance controller to have virtual pose modulation out of force/torque information.

-------------------------— 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 postion and orientation both with UI unit (meter, radian) to calculate impedance force.

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

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

Definition at line 377 of file KeypointsAdmittanceController.cpp.

Here is the call graph for this function:

◆ updateControlStatus()

bool updateControlStatus	(	const Config &	cfg,
		const IceUtil::Time &	timeSinceLastIteration,
		std::vector< const SensorValue1DoFActuatorTorque * >	torqueSensors,
		std::vector< const SensorValue1DoFActuatorVelocity * >	velocitySensors,
		std::vector< const SensorValue1DoFActuatorPosition * >	positionSensors
	)

check size and value

--------------------------— get target status from config ------------------------------------------—

update keypoints related variables

--------------------------— 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 224 of file KeypointsAdmittanceController.cpp.

Here is the call graph for this function: