6#define _USE_MATH_DEFINES
23#define DEG2RAD M_PI/180.0
37 memset(t, 0,
sizeof(T) * size);
42 std::enable_if_t<!std::is_trivial<T>::value,
int> = 0>
96 yCError(
FAKE_LASER_MOTORS) <<
"positionMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
105 for (
int j = 0, index = 0; j <
m_njoints; j++, index++)
121 yCError(
FAKE_LASER_MOTORS) <<
"relativeMoveRaw: skipping command because joint " << j <<
" is not in VOCAB_CM_POSITION mode";
130 for (
int j = 0, index = 0; j <
m_njoints; j++, index++)
149 for (
int j = 0, index = 0; j <
m_njoints; j++, index++)
171 for (
int j = 0, index = 0; j <
m_njoints; j++, index++)
203 for (
int j = 0, index = 0; j <
m_njoints; j++, index++)
209 else if (accs[j] < -1
e6)
288 for (
int j = 0; j < joints.size(); j++)
314 for (
int i = 0; i < joints.size(); i++)
345 for (
int i = 0; i < nj; ++i)
458 for (
int j = 0; j <
n_joint; j++)
468 for (
int j = 0; j <
n_joint; j++)
478 for (
int j = 0; j <
n_joint; j++)
488 for (
int j = 0; j <
n_joint; j++)
490 ret = ret &&
stopRaw(joints[j]);
497 for (
int j = 0; j <
n_joint; j++)
503 for (
int j = 0; j <
n_joint; j++)
513 for (
int j = 0; j <
n_joint; j++)
526 for (
int j = 0; j < joints.size(); j++)
538 for (
int j = 0; j <
n_joint; j++)
553 yCWarning(
FAKE_LASER_MOTORS) <<
"getTargetPosition: Joint " << axis <<
" is not in POSITION mode, therefore the value returned by " <<
554 "this call is for reference only and may not reflect the actual behaviour of the motor/firmware.";
572 for (
int i = 0; i < nj; i++)
588 yCError(
FAKE_LASER_MOTORS) <<
"velocityMoveRaw: skipping command because board " <<
" joint " << j <<
" is not in VOCAB_CM_VELOCITY mode";
606 for (
int i = 0; i <
n_joint; i++)
632 for (
int i = 0; i < nj; i++)
void checkAndDestroy(T *&p)
void checkAndDestroy(T *&p)
const yarp::os::LogComponent & FAKE_LASER_MOTORS()
T * allocAndCheck(int size)
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
double * _posCtrl_references
yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue getTargetVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode) override
yarp::dev::ReturnValue checkMotionDoneRaw(bool &flag) override
Check if the current trajectory all joints is terminated.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode) override
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getControlModesRaw(std::vector< yarp::dev::ControlModeEnum > &mode) override
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
yarp::dev::ReturnValue getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &j, const std::vector< yarp::dev::SelectableControlModeEnum > &mode) override
yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::JointTypeEnum * _jointType
yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds) override
Set reference speed on all joints.
std::vector< yarp::dev::SelectableControlModeEnum > * _availableControlModes
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue stopRaw() override
Stop motion, multiple joints.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::os::Stamp m_timestamp
A mini-server for performing network communication in the background.
double getTime() const
Get the time stamp.
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
SelectableControlModeEnum
@ VOCAB_CM_VELOCITY_DIRECT
@ VOCAB_CM_POSITION_DIRECT
The main, catch-all namespace for YARP.