26#define NEW_JSTATUS_STRUCT 1
27#define VELOCITY_WATCHDOG 0.1
28#define OPENLOOP_WATCHDOG 0.1
29#define PWM_CONSTANT 0.1
37 std::lock_guard lock(_mutex);
39 for (
int i=0;i <_njoints ;i++)
66 std::ostringstream oss;
73bool FakeMotionControlMicro::extractGroup(
Bottle &input,
Bottle &out,
const std::string &
key1,
const std::string &txt,
int size)
84 if(
tmp.size()!=(
size_t) size)
122 _axisName =
new std::string[nj];
130bool FakeMotionControlMicro::dealloc()
155 _angleToEncoder(nullptr),
156 _encodersStamp (nullptr),
158 _jointType (nullptr),
159 useRawEncoderData (
false),
162 verbose (VERY_VERBOSE)
166 verbosewhenok = (
tmp !=
"") ? (
bool)yarp::conf::numeric::from_string<int>(
tmp) :
184 for(
int i=0; i<_njoints; i++)
230 bool init = this->
start();
261 for (i = 0; i < _njoints; i++) {
275 for (i = 0; i < _njoints; i++) {
276 _axisName[_axisMap[i]] =
"joint" +
toString(i);
288 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
291 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
295 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_UNKNOWN;
303 for (i = 0; i < _njoints; i++) {
304 _jointType[_axisMap[i]] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
323 for (i = 0; i < _njoints; i++) {
324 _angleToEncoder[i] = 1; }
333 std::lock_guard lock(_mutex);
348void FakeMotionControlMicro::cleanup()
389 for(
int j=0; j< _njoints; j++)
408 for(
int j=0; j< _njoints; j++)
426 for(
int j=0; j< _njoints; j++)
439 for (
int i = 0; i < _njoints; i++) {
440 stamps[i] = _encodersStamp[i];
450 *stamp = _encodersStamp[j];
503 for(
int j=0; j< _njoints; j++)
520 for(
int j=0; j< _njoints; j++)
536 for(
int j=0; j< _njoints; j++)
547 for (
int i = 0; i < _njoints; i++) {
548 stamps[i] = _encodersStamp[i];
559 *stamp = _encodersStamp[m];
576 type = _jointType[j];
583 fault = _hwfault_code[j];
584 message = _hwfault_message[j];
void checkAndDestroy(T *&p)
std::string toString(const T &value)
convert an arbitrary type to string.
static bool NOT_YET_IMPLEMENTED(const char *txt)
static bool DEPRECATED(const char *txt)
std::string toString(const T &value)
convert an arbitrary type to string.
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
std::vector< int > m_GENERAL_AxisMap
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::vector< std::string > m_GENERAL_AxisName
std::vector< std::string > m_GENERAL_AxisType
std::vector< int > m_GENERAL_Encoder
void threadRelease() override
Release method.
yarp::dev::ReturnValue getMotorEncoderSpeedsRaw(double *spds) override
Read the instantaneous speed of all motor encoders.
yarp::dev::ReturnValue setMotorEncoderRaw(int j, double val) override
Set the value of the motor encoder for a given motor.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getMotorEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all motor encoders.
bool alloc(int njoints)
Allocated buffers.
yarp::dev::ReturnValue getMotorEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous position of all motor encoders.
bool fromConfig(yarp::os::Searchable &config)
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getMotorEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of a motor encoder.
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getMotorEncodersRaw(double *encs) override
Read the position of all motor encoders.
yarp::dev::ReturnValue getNumberOfMotorEncodersRaw(int *num) override
Get the number of available motor encoders.
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue resetMotorEncoderRaw(int j) override
Reset motor encoder, single motor.
yarp::dev::ReturnValue setMotorEncodersRaw(const double *vals) override
Set the value of all motor encoders.
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
void run() override
Loop function.
yarp::dev::ReturnValue getMotorEncoderRaw(int j, double *v) override
Read the value of a motor encoder.
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue resetMotorEncodersRaw() override
Reset motor encoders.
void resizeBuffers()
Resize previously allocated buffers.
~FakeMotionControlMicro()
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
Read the instantaneous position of a motor encoder.
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue getMotorEncoderCountsPerRevolutionRaw(int m, double *v) override
Gets number of counts per revolution for motor encoder m.
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getMotorEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of a motor encoder.
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
virtual bool initialised()
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue getLastJointFaultRaw(int j, int &fault, std::string &message) override
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::ReturnValue setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
Sets number of counts per revolution for motor encoder m.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
A simple collection of objects that can be described and transmitted in a portable way.
Bottle & findGroup(const std::string &key) const override
Gets a list corresponding to a given keyword.
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
void zero()
Set to zero the elements of the vector.
void resize(size_t size)
Resize the vector.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
std::string get_string(const std::string &key, bool *found=nullptr)
Read a string from an environment variable.
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.