/*! 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 ; } }