/*!
Send to the controller a velocity.
\param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
and camera frame (vpRobot::CAMERA_FRAME) are implemented.
\param v : Velocity to apply to the robot.
- In the camera frame, this velocity is represented by a vector of dimension 6
\f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
{\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
- In articular, this velocity is represented by a 6 dimension vector \f$
\dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
here set to identity.
We use the exponential map (vpExponentialMap) to update the camera location.
Sampling time can be set using setSamplingTime().
\sa setSamplingTime()
*/
void
vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame,
const vpColVector &v)
{
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
}
switch (frame)
{
case vpRobot::ARTICULAR_FRAME:
case vpRobot::CAMERA_FRAME: {
vpColVector v_max(6);
for (unsigned int i=0; i<3; i++)
v_max[i] = getMaxTranslationVelocity();
for (unsigned int i=3; i<6; i++)
v_max[i] = getMaxRotationVelocity();
vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
setRobotFrame(frame);
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 reference frame:"
"functionality not implemented");
break ;
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 ;
}
}
/*!
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 ;
}
}
