/*!
Operator that allows to multiply a skew transformation matrix by a
column vector.
\param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$.
For example, this operator can be used to convert a force/torque skew from sensor
frame into the probe frame :
\f$^p{\bf H} = ^p{\bf F}_s \; ^s{\bf H}\f$
The example below shows how to handle that transformation.
\code
#include <visp/vpConfig.h>
#include <visp/vpRobotViper850.h>
#include <visp/vpColVector.h>
#include <visp/vpForceTwistMatrix.h>
int main()
{
// Get the force/torque measures from the sensor
vpColVector sH(6); // Force/torque measures given by the sensor
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
robot.getForceTorque(sH); // Get the force/torque measures
#endif
// Set the transformation from sensor frame to the probe frame
vpHomogeneousMatrix pMs;
pMs[2][3] = -0.262; // tz only
// Set the force/torque twist transformation
vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame
// Compute the resulting force/torque in the probe frame
vpColVector pH(6); // Force/torque in the probe frame
pH = pFs * sH;
return 0;
}
\endcode
\exception vpMatrixException::incorrectMatrixSizeError If \f$ \bf H \f$is not a 6
dimension vector.
*/
vpColVector
vpForceTwistMatrix::operator*(const vpColVector &H) const
{
vpColVector Hout(6);
if (6 != H.getRows())
{
vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vector multiply") ;
throw(vpMatrixException::incorrectMatrixSizeError) ;
}
Hout = 0.0;
for (unsigned int i=0;i<6;i++) {
for (unsigned int j=0;j<6;j++) {
Hout[i]+=rowPtrs[i][j] * H[j];
}
}
return Hout ;
}
/*!
Operator that allows to multiply a skew transformation matrix by a
column vector.
\param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$.
For example, this operator can be used to convert a force/torque skew from sensor
frame into the probe frame :
\f$^p{\bf H} = ^p{\bf F}_s \; ^s{\bf H}\f$
The example below shows how to handle that transformation.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpForceTwistMatrix.h>
int main()
{
// Get the force/torque measures from the sensor
vpColVector sH(6); // Force/torque measures given by the sensor
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
robot.getForceTorque(sH); // Get the force/torque measures
#endif
// Set the transformation from sensor frame to the probe frame
vpHomogeneousMatrix pMs;
pMs[2][3] = -0.262; // tz only
// Set the force/torque twist transformation
vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame
// Compute the resulting force/torque in the probe frame
vpColVector pH(6); // Force/torque in the probe frame
pH = pFs * sH;
return 0;
}
\endcode
\exception vpException::dimensionError If \f$ \bf H \f$is not a 6
dimension vector.
*/
vpColVector
vpForceTwistMatrix::operator*(const vpColVector &H) const
{
vpColVector Hout(6);
if (6 != H.getRows()) {
throw (vpException(vpException::dimensionError,
"Cannot multiply a (6x6) force/torque twist matrix by a %d dimension column vector",
H.getRows()));
}
Hout = 0.0;
for (unsigned int i=0;i<6;i++) {
for (unsigned int j=0;j<6;j++) {
Hout[i]+=rowPtrs[i][j] * H[j];
}
}
return Hout ;
}
