39 #include <visp3/core/vpDebug.h>
40 #include <visp3/robot/vpRobot.h>
41 #include <visp3/robot/vpRobotException.h>
54 : stateRobot(
vpRobot::STATE_STOP), frameRobot(
vpRobot::CAMERA_FRAME),
55 maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
56 eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
62 : stateRobot(
vpRobot::STATE_STOP), frameRobot(
vpRobot::CAMERA_FRAME),
63 maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
64 eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
88 stateRobot = robot.stateRobot;
89 frameRobot = robot.frameRobot;
100 for (
int i = 0; i <
nDof; i++) {
165 unsigned int size = v_in.
size();
166 if (size != v_max.
size())
170 for (
unsigned int i = 0; i < size; i++) {
171 double v_i = fabs(v_in[i]);
172 double v_max_i = fabs(v_max[i]);
175 double scale_i = v_max_i / v_i;
180 std::cout <<
"Excess velocity " << v_in[i] <<
" axis nr. " << i << std::endl;
185 v_sat = v_in * scale;
203 stateRobot = newState;
209 frameRobot = newFrame;
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
@ dimensionError
Bad dimension.
Error that can be emited by the vpRobot class and its derivates.
Class that defines a generic virtual robot.
int nDof
number of degrees of freedom
static const double maxTranslationVelocityDefault
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
vpRobot & operator=(const vpRobot &robot)
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
static const double maxRotationVelocityDefault
int areJointLimitsAvailable
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)=0
Get the robot position (frame has to be specified).
int fJeAvailable
is the robot Jacobian expressed in the robot reference frame available
double getMaxRotationVelocity(void) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double maxTranslationVelocity
int eJeAvailable
is the robot Jacobian expressed in the end-effector frame available
double getMaxTranslationVelocity(void) const
double maxRotationVelocity
void setMaxRotationVelocity(double maxVr)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
void setMaxTranslationVelocity(double maxVt)