28 #ifndef __iCubSimulationControlh__
29 #define __iCubSimulationControlh__
34 #include <yarp/os/Bottle.h>
36 #include <yarp/sig/Image.h>
37 #include <yarp/sig/Matrix.h>
38 #include <yarp/dev/DeviceDriver.h>
39 #include <yarp/dev/PolyDriver.h>
40 #include <yarp/dev/ControlBoardInterfaces.h>
41 #include <yarp/dev/ControlBoardInterfacesImpl.h>
47 #define MODE_IDLE 0x00
48 #define MODE_POSITION 0x01
49 #define MODE_VELOCITY 0x02
50 #define MODE_TORQUE 0x03
51 #define MODE_IMPEDANCE_POS 0x04
52 #define MODE_IMPEDANCE_VEL 0x05
54 #define MODE_CURRENT 0x60
55 #define MODE_MIXED VOCAB_CM_MIXED
56 #define MODE_FORCE_IDLE VOCAB_CM_FORCE_IDLE
57 #define MODE_HW_FAULT VOCAB_CM_HW_FAULT
58 #define MODE_CALIBRATING VOCAB_CM_CALIBRATING
59 #define MODE_CALIB_DONE VOCAB_CM_CALIB_DONE
60 #define MODE_NOT_CONFIGURED VOCAB_CM_NOT_CONFIGURED
61 #define MODE_CONFIGURED VOCAB_CM_CONFIGURED
62 #define MODE_UNKNOWN VOCAB_CM_UNKNOWN
74 class iCubSimulationControl;
80 public IPositionControlRaw,
81 public ImplementPositionControl,
82 public IVelocityControlRaw,
83 public ImplementVelocityControl,
84 public ITorqueControlRaw,
85 public ImplementTorqueControl,
86 public IAmplifierControlRaw,
87 public ImplementAmplifierControl,
88 public IControlCalibrationRaw,
89 public ImplementControlCalibration,
90 public IControlLimitsRaw,
91 public ImplementControlLimits,
92 public IControlModeRaw,
93 public ImplementControlMode,
94 public IInteractionModeRaw,
95 public ImplementInteractionMode,
96 public IPidControlRaw,
97 public ImplementPidControl,
98 public IEncodersTimedRaw,
99 public ImplementEncodersTimed,
100 public IMotorEncodersRaw,
101 public ImplementMotorEncoders,
103 public ImplementMotor,
104 public IPositionDirectRaw,
105 public ImplementPositionDirect,
106 public IRemoteVariablesRaw,
107 public ImplementRemoteVariables,
109 public ImplementAxisInfo,
110 public IPWMControlRaw,
111 public ImplementPWMControl,
112 public ICurrentControlRaw,
113 public ImplementCurrentControl
118 int ControlModes_yarp2iCubSIM(
int yarpMode);
119 int ControlModes_iCubSIM2yarp(
int iCubMode);
138 virtual bool open(yarp::os::Searchable& config);
140 virtual bool close(
void);
150 virtual bool setPidRaw(
const PidControlTypeEnum& pidtype,
int j,
const Pid &pid)
override;
151 virtual bool setPidsRaw(
const PidControlTypeEnum& pidtype,
const Pid *pids)
override;
152 virtual bool setPidReferenceRaw(
const PidControlTypeEnum& pidtype,
int j,
double ref)
override;
153 virtual bool setPidReferencesRaw(
const PidControlTypeEnum& pidtype,
const double *refs)
override;
154 virtual bool setPidErrorLimitRaw(
const PidControlTypeEnum& pidtype,
int j,
double limit)
override;
155 virtual bool setPidErrorLimitsRaw(
const PidControlTypeEnum& pidtype,
const double *limits)
override;
156 virtual bool getPidErrorRaw(
const PidControlTypeEnum& pidtype,
int j,
double *err)
override;
157 virtual bool getPidErrorsRaw(
const PidControlTypeEnum& pidtype,
double *errs)
override;
158 virtual bool getPidOutputRaw(
const PidControlTypeEnum& pidtype,
int j,
double *
out)
override;
159 virtual bool getPidOutputsRaw(
const PidControlTypeEnum& pidtype,
double *outs)
override;
160 virtual bool getPidRaw(
const PidControlTypeEnum& pidtype,
int j, Pid *pid)
override;
161 virtual bool getPidsRaw(
const PidControlTypeEnum& pidtype, Pid *pids)
override;
162 virtual bool getPidReferenceRaw(
const PidControlTypeEnum& pidtype,
int j,
double *ref)
override;
163 virtual bool getPidReferencesRaw(
const PidControlTypeEnum& pidtype,
double *refs)
override;
164 virtual bool getPidErrorLimitRaw(
const PidControlTypeEnum& pidtype,
int j,
double *limit)
override;
166 virtual bool resetPidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
167 virtual bool disablePidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
168 virtual bool enablePidRaw(
const PidControlTypeEnum& pidtype,
int j)
override;
169 virtual bool setPidOffsetRaw(
const PidControlTypeEnum& pidtype,
int j,
double v)
override;
170 virtual bool isPidEnabledRaw(
const PidControlTypeEnum& pidtype,
int j,
bool* enabled)
override;
175 virtual bool getAxes(
int *ax)
override;
190 virtual bool stopRaw(
int j)
override;
191 virtual bool stopRaw()
override;
201 virtual bool stopRaw(
const int n_joint,
const int *
joints)
override;
269 virtual bool getPWMRaw(
int j,
double* val)
override;
287 virtual bool getAxisNameRaw(
int axis, std::string& name)
override;
288 virtual bool getJointTypeRaw(
int axis, yarp::dev::JointTypeEnum& type)
override;
369 void compute_mot_pos_from_jnt_pos(
double *mot_pos,
const double *jnt_pos,
int size_joints);
370 void compute_mot_vel_and_acc (
double *mot_vel,
double *mot_acc,
const double *mot_pos,
int size_joints);
371 void compute_jnt_vel_and_acc (
double *jnt_vel,
double *jnt_acc,
const double *jnt_pos,
int size_joints);
Route control for the robots controlled joints to their implementation.
Adaptive window linear fitting to estimate the first derivative.
Adaptive window quadratic fitting to estimate the second derivative.
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp)
iCub::ctrl::AWLinEstimator * linEstMot
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool getEncodersRaw(double *encs) override
iCub::ctrl::AWQuadEstimator * quadEstJnt
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool getRefDutyCycleRaw(int j, double *v) override
double * estimated_jnt_vel
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool setEncodersRaw(const double *vals) override
virtual bool resetEncoderRaw(int j) override
yarp::sig::Matrix kinematic_mj
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
virtual bool getTargetPositionsRaw(double *refs) override
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
virtual bool resetMotorEncoderRaw(int m) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool velocityMoveRaw(int j, double sp) override
for each axis
virtual bool setRefAccelerationsRaw(const double *accs) override
MotorTorqueParameters * motor_torque_params
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool setPWMLimitRaw(int j, const double val) override
virtual bool setRefTorqueRaw(int, double) override
virtual bool getNumberOfMotorEncodersRaw(int *num) override
double * newtonsToSensor
encoder zeros
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool getMaxCurrentRaw(int j, double *val) override
virtual bool getDutyCyclesRaw(double *v) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getPWMRaw(int j, double *val) override
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
static const int positionUpdateRate
double * current_ampere_ref
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool getControlModeRaw(int j, int *mode) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getRefTorquesRaw(double *) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool setControlModeRaw(const int j, const int mode) override
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getAmpStatusRaw(int *st) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis pointer with 1 double arg
virtual bool setMotorEncodersRaw(const double *vals) override
virtual bool getTorquesRaw(double *) override
virtual bool getTemperaturesRaw(double *vals) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
double * current_mot_torques
virtual bool calibrationDoneRaw(int j) override
virtual bool getTorqueRangesRaw(double *, double *) override
virtual bool positionMoveRaw(int j, double ref) override
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getCurrentsRaw(double *vals) override
double * current_jnt_torques
virtual bool getTorqueRangeRaw(int, double *, double *) override
virtual bool disableAmpRaw(int j) override
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool enableAmpRaw(int j) override
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getNominalCurrentRaw(int m, double *val) override
virtual bool getAxisNameRaw(int axis, std::string &name) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getTorqueRaw(int, double *) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
virtual bool checkMotionDoneRaw(bool *flag) override
yarp::dev::PolyDriver joints
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getRefAccelerationsRaw(double *accs) override
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
virtual bool getCurrentRaw(int j, double *val) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool getRefCurrentsRaw(double *t) override
double * ref_command_positions
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool getRefSpeedsRaw(double *spds) override
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool getRefPositionsRaw(double *refs) override
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setRefDutyCyclesRaw(const double *v) override
double * estimated_mot_acc
virtual bool setPositionRaw(int j, double ref) override
virtual bool getMotorEncoderRaw(int m, double *v) override
virtual bool setRefTorquesRaw(const double *) override
virtual bool getControlModesRaw(int *modes) override
double * estimated_jnt_acc
double * ref_command_speeds
virtual ~iCubSimulationControl()
Destructor.
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool stopRaw() override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
virtual bool setRefCurrentsRaw(const double *t) override
virtual bool getEncodersTimedRaw(double *encs, double *stamps)
virtual bool resetEncodersRaw() override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool getDutyCycleRaw(int j, double *v) override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool setEncoderRaw(int j, double val) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
iCub::ctrl::AWLinEstimator * linEstJnt
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
double * estimated_mot_vel
iCubSimulationControl()
Default constructor.
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
virtual bool resetMotorEncodersRaw() override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool getRefTorqueRaw(int, double *) override
virtual bool setMotorEncoderRaw(int m, const double val) override
iCub::ctrl::AWQuadEstimator * quadEstMot
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getPWMLimitRaw(int j, double *val) override
Copyright (C) 2008 RobotCub Consortium.