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

Inheritance diagram for TSCtrlRtStatus:

Public Member Functions
void	reset (size_t numDoF, const std::vector< size_t > &jointIDTorqueMode, const std::vector< size_t > &jointIDVelocityMode) override

void	rtPreActivate (const Eigen::Matrix4f &currentTCPPose)

Public Member Functions inherited from RobotStatus
virtual	~RobotStatus ()=default

Public Attributes
Eigen::Vector6f	acc

Eigen::Vector6f	cartesianVelTarget
	for impedance control

Eigen::Vector6f	currentForceTorque
	force torque

Eigen::Matrix4f	currentPose

Eigen::Vector6f	currentTwist

Eigen::VectorXf	desiredJointPosition

Eigen::VectorXf	desiredJointTorque
	targets

Eigen::VectorXf	desiredJointVelocity

Eigen::Matrix4f	desiredPose

Eigen::Matrix4f	desiredPoseUnsafe

Eigen::Vector6f	forceImpedance
	task space variables (command in operation space)

bool	forceTorqueSafe = false

Eigen::MatrixXf	I
	for computing nullspace projection

Eigen::MatrixXd	inertia
	inertia Note, the inertia variables are only used for collision avoidance controllers, as the inertia is obtained from the simox robot that exists only in collision avoidance controllers TODO, use simox to obtain inertia

Eigen::MatrixXf	inertiaInv

Eigen::MatrixXf	inertiaInvVelCtrlJoints

Eigen::MatrixXd	inertiaVelCtrlJoints

Eigen::MatrixXf	IVelCtrlJoint

Eigen::MatrixXf	jacobi
	others jacobi matrix [6, nDoF]

Eigen::MatrixXf	jpinv
	pseudo inverse of the jacobi [nDoF, 6], for vel control

Eigen::MatrixXf	jtpinv
	pseudo inverse of the jacobi transpose [6, nDoF], for torque control

float	lambda = 2.0f
	damping term for pseudo inverse of jacobian

Eigen::VectorXf	nullspaceTorque

Eigen::VectorXf	nullspaceVelocity

Eigen::AngleAxisf	oriDiffAngleAxis

Eigen::Vector6f	pos

Eigen::Matrix3f	poseDiffMatAdm
	intermediate results for admittance controller

Eigen::Matrix3f	poseDiffMatImp
	intermediate results

Eigen::Vector6f	poseErrorAdm

Eigen::Vector6f	poseErrorImp

Eigen::VectorXf	qdAcc

Eigen::VectorXf	qdPos

Eigen::VectorXf	qdVel

Eigen::VectorXf	qTemp

Eigen::VectorXf	qvelFiltered
	for velocity control

bool	rtSafe = false

bool	rtTargetSafe = false

Eigen::Vector6f	safeFTGuardOffset

Eigen::Vector6f	vel

Eigen::Vector6f	virtualAcc

Eigen::Matrix4f	virtualPose

Eigen::Vector6f	virtualVel

Public Attributes inherited from RobotStatus
double	accumulateTime = 0.0

double	deltaT = 0

Eigen::Matrix4f	globalPose

std::vector< size_t >	jointIDTorqueMode

std::vector< size_t >	jointIDVelocityMode

Eigen::VectorXf	jointPosition

Eigen::VectorXf	jointTorque

Eigen::VectorXf	jointVelocity

size_t	nDoF = 0

size_t	nDoFTorque = 0

size_t	nDoFVelocity = 0

Member Function Documentation

◆ reset()

void reset	(	size_t	numDoF,
		const std::vector< size_t > &	jointIDTorqueMode,
		const std::vector< size_t > &	jointIDVelocityMode )

overridevirtual

This function should be called BEFORE the RT thread. Usually in constructor of the controller

targets

force torque

task space variables (command in operation space)

current status

intermediate results

intermediate results for admittance controller

inertia

others

Reimplemented from RobotStatus.

◆ rtPreActivate()

void rtPreActivate ( const Eigen::Matrix4f & currentTCPPose )

!!!! TO BE UPDATED IN PRE-ACTIVATE !!!! only then the up-to-date robot joint information is available

Member Data Documentation

◆ acc

Eigen::Vector6f acc

◆ cartesianVelTarget

Eigen::Vector6f cartesianVelTarget

for impedance control

◆ currentForceTorque

Eigen::Vector6f currentForceTorque

force torque

◆ currentPose

Eigen::Matrix4f currentPose

◆ currentTwist

Eigen::Vector6f currentTwist

◆ desiredJointPosition

Eigen::VectorXf desiredJointPosition

◆ desiredJointTorque

Eigen::VectorXf desiredJointTorque

targets

◆ desiredJointVelocity

Eigen::VectorXf desiredJointVelocity

◆ desiredPose

Eigen::Matrix4f desiredPose

◆ desiredPoseUnsafe

Eigen::Matrix4f desiredPoseUnsafe

◆ forceImpedance

Eigen::Vector6f forceImpedance

task space variables (command in operation space)

◆ forceTorqueSafe

bool forceTorqueSafe = false

◆ I

Eigen::MatrixXf I

for computing nullspace projection

◆ inertia

Eigen::MatrixXd inertia

inertia Note, the inertia variables are only used for collision avoidance controllers, as the inertia is obtained from the simox robot that exists only in collision avoidance controllers TODO, use simox to obtain inertia

◆ inertiaInv

Eigen::MatrixXf inertiaInv

◆ inertiaInvVelCtrlJoints

Eigen::MatrixXf inertiaInvVelCtrlJoints

◆ inertiaVelCtrlJoints

Eigen::MatrixXd inertiaVelCtrlJoints

◆ IVelCtrlJoint

Eigen::MatrixXf IVelCtrlJoint

◆ jacobi

Eigen::MatrixXf jacobi

others jacobi matrix [6, nDoF]

◆ jpinv

Eigen::MatrixXf jpinv

pseudo inverse of the jacobi [nDoF, 6], for vel control

◆ jtpinv

Eigen::MatrixXf jtpinv

pseudo inverse of the jacobi transpose [6, nDoF], for torque control

◆ lambda

float lambda = 2.0f

damping term for pseudo inverse of jacobian

◆ nullspaceTorque

Eigen::VectorXf nullspaceTorque

◆ nullspaceVelocity

Eigen::VectorXf nullspaceVelocity

◆ oriDiffAngleAxis

Eigen::AngleAxisf oriDiffAngleAxis

◆ pos

Eigen::Vector6f pos

◆ poseDiffMatAdm

Eigen::Matrix3f poseDiffMatAdm

intermediate results for admittance controller

◆ poseDiffMatImp

Eigen::Matrix3f poseDiffMatImp

intermediate results

◆ poseErrorAdm

Eigen::Vector6f poseErrorAdm

◆ poseErrorImp

Eigen::Vector6f poseErrorImp

◆ qdAcc

Eigen::VectorXf qdAcc

◆ qdPos

Eigen::VectorXf qdPos

◆ qdVel

Eigen::VectorXf qdVel

◆ qTemp

Eigen::VectorXf qTemp

◆ qvelFiltered

Eigen::VectorXf qvelFiltered

for velocity control