Go to the documentation of this file.
3 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
4 #include <VirtualRobot/MathTools.h>
15 const VirtualRobot::RobotNodeSetPtr& rtRns,
16 const std::vector<size_t>& torqueControlledIndex,
17 const std::vector<size_t>& velocityControlledIndex)
29 rtTCP = rtRns->getTCP();
31 ik.reset(
new VirtualRobot::DifferentialIK(
32 rtRns, rtRns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
105 ik->getJacobianMatrix(
rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
111 rtStatus.
jpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.
jacobi, lambda);
130 for (
size_t i = 0; i < 6; i++)
145 c.kdCartesianVel.head(3).cwiseProduct(rtStatus.
currentTwist.head(3));
151 c.kdCartesianVel.tail(3).cwiseProduct(rtStatus.
currentTwist.tail(3));
Eigen::VectorXf desiredJointTorques
targets
std::vector< size_t > jointIDTorqueMode
common::ft::FTSensor ftsensor
Eigen::Vector6f cartesianVelTarget
task space variables
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
void updateFT(const common::ft::arondto::FTConfig &c, RtStatus &rtStatus)
void initialize(const VirtualRobot::RobotNodeSetPtr &rns, const VirtualRobot::RobotNodeSetPtr &rtRns, const std::vector< size_t > &torqueControlledIndex, const std::vector< size_t > &velocityControlledIndex)
Eigen::Vector6f currentForceTorque
force torque
std::vector< float > jointVelocity
Eigen::MatrixXf jacobi
others
double clamp(double x, double a, double b)
std::vector< size_t > jointIDVelocityMode
VirtualRobot::RobotNodePtr tcp
void Identity(MatrixXX< N, N, T > *a)
void run(Config &c, RtStatus &robotStatus)
void preactivateInit(const VirtualRobot::RobotNodeSetPtr &rns)
Eigen::Vector6f currentTwist
std::atomic_bool calibrated
This file is part of ArmarX.
Eigen::VectorXf qpos
current status
std::vector< float > jointPosition
arondto::ZeroTorqueOrVelocityControllerConfig Config
you can set the following values from outside of the rt controller via Ice interfaces
VirtualRobot::RobotNodePtr rtTCP
Eigen::Vector6f & getFilteredForceTorque(const FTConfig &data)
bool calibrate(const FTConfig &c, double deltaT)
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Eigen::VectorXf qvelFiltered
internal status of the controller, containing intermediate variables, mutable targets
Eigen::VectorXf desiredJointVelocity
Eigen::Vector6f & compensateTCPGravity(const FTConfig &data)