/*!
Compute end return the dot product of two column vectors:
\f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of both vectors.
\exception vpException::dimensionError If the vector dimension differ.
*/
double
vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{
if (a.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute the dot product: first vector empty")) ;
}
if (b.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute the dot product: second vector empty")) ;
}
if (a.size() != b.size()) {
throw(vpException(vpException::dimensionError,
"Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
a.size(), b.size()));
}
double *ad = a.data ; double *bd = b.data ;
double c = 0 ;
for (unsigned int i=0 ; i < a.getRows() ; i++)
c += *(ad++)* *(bd++) ;
// vpMatrix c = (a.t() * b);
// return c[0][0];
return c ;
}
/*!
Insert a column vector.
\param i : Index of the first element to introduce. This index starts from 0.
\param v : Column vector to insert.
The following example shows how to use this function:
\code
#include <visp3/core/vpColVector.h>
int main()
{
vpColVector v(4);
for (unsigned int i=0; i < v.size(); i++)
v[i] = i;
std::cout << "v: " << v.t() << std::endl;
vpColVector w(2);
for (unsigned int i=0; i < w.size(); i++)
w[i] = i+10;
std::cout << "w: " << w.t() << std::endl;
v.insert(1, w);
std::cout << "v: " << v.t() << std::endl;
}
\endcode
It produces the following output:
\code
v: 0 1 2 3
w: 10 11
v: 0 10 11 3
\endcode
*/
void vpColVector::insert(unsigned int i, const vpColVector &v)
{
if (i+v.size() > this->size())
throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
for (unsigned int j=0; j < v.size(); j++)
(*this)[i+j] = v[j];
}
/*!
Operator that performs the dot product between two column vectors.
\exception vpException::dimensionError If the vector dimension differ.
\sa dotProd()
*/
double
vpColVector::operator*(const vpColVector &v) const
{
if (size() != v.size()) {
throw(vpException(vpException::dimensionError,
"Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
size(), v.size()));
}
double r = 0 ;
for (unsigned int i=0;i<rowNum;i++)
r += (*this)[i] * v[i];
return r;
}
/*!
Compute the median value of all the elements of the vector.
*/
double
vpColVector::median(const vpColVector &v)
{
if (v.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute column vector median: vector empty")) ;
}
std::vector<double> vectorOfDoubles(v.size());
for(unsigned int i = 0; i < v.size(); i++) {
vectorOfDoubles[i] = v[i];
}
return vpMath::getMedian(vectorOfDoubles);
}
void Eigenfaces::getFaceWithCoordinates(const vpColVector & coordinates, vpMatrix & face) /*const*/ {
vpColVector cface = _meanFace.stackRows().t();
for (unsigned int i = 0; i < coordinates.size(); i++)
cface += _eigenfaces.getCol(i) * coordinates[i];
face = cface.t().reshape(_iheight, _iwidth);
}
/*! Copy constructor from a 3-dimension vector. */
vpRzyzVector::vpRzyzVector(const vpColVector &rzyz)
: vpRotationVector (3)
{
if (rzyz.size() != 3) {
throw(vpException(vpException::dimensionError, "Cannot construct a R-zyz vector from a %d-dimension col vector", rzyz.size()));
}
for (unsigned int i=0; i< 3; i++)
data[i] = rzyz[i];
}
//! Constructor from a 4-dimension vector of doubles.
vpQuaternionVector::vpQuaternionVector(const vpColVector &q)
: vpRotationVector(4)
{
if (q.size() != 4) {
throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
}
for (unsigned int i=0; i<4; i++)
data[i] = q[i];
}
std::vector<double> vispToStdVector(const vpColVector& vpVector,
int initialPose,
int length)
{
if ( length == - 1)
length = vpVector.size() - initialPose;
std::vector<double> stdVector(length);
for (int i = initialPose; i < initialPose + length; ++i)
stdVector[i - initialPose] = vpVector[i];
return stdVector;
}
/*!
* Set articular force.
* Works in mode COMMAND_TYPE_ARTICULAR_IMPEDANCE that need to be set with setCommandType().
* \param articularForce :
*/
void vpVirtuose::setArticularForce (const vpColVector &articularForce)
{
init();
if (articularForce.size() != 6) {
throw(vpException(vpException::dimensionError,
"Cannot apply an articular force feedback (dim %d) to the haptic device that is not 6-dimension",
articularForce.size()));
}
float articular_force[6];
for(unsigned int i=0; i<6; i++)
articular_force[i] = articularForce[i];
if (virtSetArticularForce(m_virtContext, articular_force)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtSetArticularForce: error code %d", err));
}
}
/*!
* Send articular (joint) velocity command to the virtuose.
* Works in COMMAND_TYPE_ARTICULAR mode that need to be set with setCommandType().
* \param articularVelocity : Six dimension joint velocity vector.
*/
void vpVirtuose::setArticularVelocity (const vpColVector &articularVelocity)
{
init();
if (articularVelocity.size() != 6) {
throw(vpException(vpException::dimensionError,
"Cannot send an articular velocity command (dim %d) to the haptic device that is not 6-dimension",
articularVelocity.size()));
}
float articular_velocity[6];
for(unsigned int i=0; i<6; i++)
articular_velocity[i] = articularVelocity[i];
if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtSetArticularVelocity: error code %d", err));
}
}
/*!
* Add a force to be applied to the virtuose (impedance effort).
* This function works in every mode.
* \param force : Is 6 component dynamic tensor (three forces and three torques) wrt virtuose end-effector
* and is expressed in the coordinates of the base frame.
*/
void vpVirtuose::addForce (vpColVector &force)
{
if (force.size() != 6) {
throw(vpException(vpException::dimensionError,
"Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
force.size()));
}
init();
float virtforce[6];
for(unsigned int i=0; i<6; i++)
virtforce[i] = force[i];
if (virtAddForce(m_virtContext, virtforce)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtAddForce: error code %d", err));
}
}
/*!
* Modify the current control speed.
* \param velocity : Velocity twist vector, where translations velocities are expressed in
* m/s and rotation velocities in rad/s.
*/
void vpVirtuose::setVelocity (vpColVector &velocity)
{
init();
if (velocity.size() != 6) {
throw(vpException(vpException::dimensionError,
"Cannot set a velocity vector (dim %d) that is not 6-dimension",
velocity.size()));
}
float speed[6];
for(unsigned int i=0; i<6; i++)
speed[i] = velocity[i];
if (virtSetSpeed(m_virtContext, speed)) {
int err = virtGetErrorCode(m_virtContext);
throw(vpException(vpException::fatalError,
"Error calling virtSetSpeed: error code %d", err));
}
}
/*!
Compute the standard deviation value of all the elements of the vector.
*/
double
vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection)
{
if (v.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute column vector stdev: vector empty")) ;
}
double mean_value = mean(v);
double sum_squared_diff = 0.0;
for(unsigned int i = 0; i < v.size(); i++) {
sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
}
double divisor = (double) v.size();
if(useBesselCorrection && v.size() > 1) {
divisor = divisor-1;
}
return std::sqrt(sum_squared_diff / divisor);
}
/*!
Set the velocity (frame has to be specified) that will be applied to the robot.
\param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
rotational velocities are implemented.
\param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
- If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
independently.
- If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
Second value is the rotational velocity in rad/s.
Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
\exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
init();
/*
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
vpERROR_TRACE ("Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
} */
if (vel.size() != 2)
{
throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
}
vpColVector vel_max(2);
vpColVector vel_sat;
if (frame == vpRobot::REFERENCE_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxRotationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
this->unlock();
}
else if (frame == vpRobot::ARTICULAR_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxTranslationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
//std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
this->unlock();
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send the robot velocity in the specified control frame");
}
}
/*!
Operation a = aHb * b.
\param b : 3 dimension vector.
*/
vpColVector
vpHomography::operator*(const vpColVector &b) const
{
if (b.size() != 3)
throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));
vpColVector a(3);
for(unsigned int i=0; i<3; i++) {
a[i] = 0.;
for(unsigned int j=0; j<3; j++)
a[i] += (*this)[i][j] * b[j];
}
return a;
}
void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out)
{
if (p.size() < 2) {
vpCTRACE << "Bad template tracker warp parameters dimension. Should never occur. " << std::endl;
throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
}
vpColVector S1(2),S2(2),S3(2);
vpColVector rS1(2),rS2(2),rS3(2);
in.getCorners(S1,S2,S3);
computeDenom(S1,p);
warpX(S1,rS1,p);
computeDenom(S2,p);
warpX(S2,rS2,p);
computeDenom(S3,p);
warpX(S3,rS3,p);
out.init(rS1,rS2,rS3);
}
/*!
Send to the controller a velocity.
\param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
\param v : Velocity vector \f$(v_x, w_z)\f$ to apply to the robot.
Depending on the velocity specified as input, the robot position is updated
using the sampling time that can be modified using setSamplingTime().
\sa setSamplingTime()
*/
void
vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame,
const vpColVector &v)
{
switch (frame)
{
case vpRobot::ARTICULAR_FRAME: {
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
}
setRobotFrame(frame);
// v is a 2 dimension vector that contains v,w
if (v.size() != 2) {
vpERROR_TRACE ("Bad dimension of the control vector");
throw vpRobotException (vpRobotException::dimensionError,
"Bad dimension of the control vector");
}
vpColVector v_max(2);
v_max[0] = getMaxTranslationVelocity();
v_max[1] = getMaxRotationVelocity();
vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
xm_ += delta_t_ * v_sat[0] * cos(theta_);
ym_ += delta_t_ * v_sat[0] * sin(theta_);
theta_ += delta_t_ * v_sat[1];
vpRotationMatrix wRe(0, 0, theta_);
vpTranslationVector wte(xm_, ym_, 0);
wMe_.buildFrom(wte, wRe);
wMc_ = wMe_ * cMe_.inverse();
break ;
}
break ;
case vpRobot::CAMERA_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the camera frame:"
"functionality not implemented");
break ;
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the articular frame:"
"functionality not implemented");
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
}
