12 #include <dynamic-graph/factory.h> 27 OpPointModifier::OpPointModifier(
const std::string &
name)
30 "OpPointModifior(" + name +
")::input(matrix)::jacobianIN"),
32 NULL,
"OpPointModifior(" + name +
")::input(matrixhomo)::positionIN"),
36 "OpPointModifior(" + name +
")::output(matrix)::jacobian"),
40 "OpPointModifior(" + name +
")::output(matrixhomo)::position"),
95 aAB = aMb.translation();
99 res.resize(6, oJa.cols());
100 for (
int j = 0; j < nq; ++j) {
102 res(0, j) = oJa(0, j) - oAB(1) * oJa(2 + 3, j) + oAB(2) * oJa(1 + 3, j);
103 res(1, j) = oJa(1, j) - oAB(2) * oJa(0 + 3, j) + oAB(0) * oJa(2 + 3, j);
104 res(2, j) = oJa(2, j) - oAB(0) * oJa(1 + 3, j) + oAB(1) * oJa(0 + 3, j);
105 for (
int i = 0;
i < 3; ++
i) {
106 res(
i + 3, j) = oJa(
i + 3, j);
137 #include <dynamic-graph/pool.h> void setTransformation(const Eigen::Matrix4d &tr)
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
std::string docDirectGetter(const std::string &name, const std::string &type)
DirectGetter< E, T > * makeDirectGetter(E &entity, T *ptr, const std::string &docString)
dynamicgraph::SignalTimeDependent< MatrixHomogeneous, int > positionSOUT
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
void signalRegistration(const SignalArray< int > &signals)
#define sotDEBUGOUT(level)
dynamicgraph::Matrix & jacobianSOUT_function(dynamicgraph::Matrix &res, const int &time)
Signal< dynamicgraph::Matrix, int > sig("matrix")
void setTransformationBySignalName(std::istringstream &cmdArgs)
#define sotDEBUGIN(level)
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
const Eigen::Matrix4d & getTransformation(void)
MatrixHomogeneous transformation
MatrixHomogeneous & positionSOUT_function(MatrixHomogeneous &res, const int &time)
virtual const Time & getTime() const
static PoolStorage * getInstance()
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
def deprecated(instructions)
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
virtual const T & accessCopy() const
dynamicgraph::SignalPtr< MatrixHomogeneous, int > positionSIN
Compute position and jacobian of a local frame attached to a joint.
SignalBase< int > & getSignal(std::istringstream &sigpath)
void addCommand(const std::string &name, command::Command *command)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier, "OpPointModifier")
virtual const Time & getTime() const
std::string docDirectSetter(const std::string &name, const std::string &type)
DirectSetter< E, T > * makeDirectSetter(E &entity, T *ptr, const std::string &docString)
dynamicgraph::SignalPtr< dynamicgraph::Matrix, int > jacobianSIN