robot.h

Go to the documentation of this file.

#ifndef __ROBOT_H__
#define __ROBOT_H__

#include <boost/array.hpp>
#include <semaphore.h>
#include <hrpModel/Body.h>
#include "hrpsys/io/iob.h"

class robot : public hrp::Body
{
public:
    robot(double dt);

    ~robot();

    bool init();

    bool servo(int jid, bool turnon);

    bool servo(const char *jname, bool turnon);

    bool power(int jid, bool turnon);

    bool power(const char *jname, bool turnon);

    void removeForceSensorOffset();

    bool loadGain();

    void startInertiaSensorCalibration();

    void startForceSensorCalibration();

    void initializeJointAngle(const char *name, const char *option);

    void oneStep();

    int readCalibState(int i);

    int readPowerState(int i);

    int readServoState(int i);

    int readServoAlarm(int i);

    int readDriverTemperature(int i);

    void readPowerStatus(double &o_voltage, double &o_current);

    void readBatteryState(unsigned int i_rank, double &o_voltage,
                          double &o_current, double &o_soc);

    void readThermometer(unsigned int i_rank, double &o_temp);

    void readJointAngles(double *o_angles);

    void readJointVelocities(double *o_velocities);

    int readJointTorques(double *o_torques);

    int readJointCommandTorques(double *o_torques);

    void readGyroSensor(unsigned int i_rank, double *o_rates);

    void readAccelerometer(unsigned int i_rank, double *o_accs);

    void readForceSensor(unsigned int i_rank, double *o_forces);

    void writeJointCommands(const double *i_commands);

    void readJointCommands(double *o_commands);

    void writeTorqueCommands(const double *i_commands);

    void writeVelocityCommands(const double *i_commands);

    void writeAccelerationCommands(const double *i_commands);

    int readPDControllerTorques(double *o_torques);

    size_t lengthOfExtraServoState(int id);

    void readExtraServoState(int id, int *state);

    typedef enum {EMG_SERVO_ERROR, EMG_FZ, EMG_SERVO_ALARM, EMG_POWER_OFF} emg_reason;

    bool checkEmergency(emg_reason &o_reason, int &o_id);


    bool checkJointCommands(const double *i_commands);

    bool setServoGainPercentage(const char *i_jname, double i_percentage);

    bool setServoTorqueGainPercentage(const char *i_jname, double i_percentage);

    bool setServoErrorLimit(const char *i_jname, double i_limit);

    bool setJointInertia(const char *i_jname, double i_mn);

    void setJointInertias(const double *i_mn);

    void enableDisturbanceObserver();

    void disableDisturbanceObserver();

    void setDisturbanceObserverGain(double gain);

    void setProperty(const char *key, const char *value);
    bool addJointGroup(const char *gname, const std::vector<std::string>& jnames);
    std::vector<double> m_servoErrorLimit;  
    double m_fzLimitRatio;
    double m_maxZmpError;
    double m_accLimit;
    double m_servoOnDelay;

    bool readDigitalInput(char *o_din);
    int lengthDigitalInput();
    bool writeDigitalOutput(const char *i_dout);
    bool writeDigitalOutputWithMask(const char *i_dout, const char *i_mask);
    int lengthDigitalOutput();
    bool readDigitalOutput(char *o_dout);

    int numBatteries();

    int numThermometers();

    bool setJointControlMode(const char *i_jname, joint_control_mode mode);
private:
    void calibrateInertiaSensorOneStep();

    void calibrateForceSensorOneStep();

    bool isBusy() const;

    bool names2ids(const std::vector<std::string> &i_names, 
                   std::vector<int> &o_ids);

    void gain_control();
    void gain_control(int id);

    int inertia_calib_counter, force_calib_counter;
    std::vector<double> gain_counter;

    std::vector< boost::array<double,3> > gyro_sum;
    std::vector< boost::array<double,3> > accel_sum;
    std::vector< boost::array<double,3> > att_sum;
    std::vector< boost::array<double,6> > force_sum;

    std::vector<double> pgain, old_pgain, default_pgain;
    std::vector<double> dgain, old_dgain, default_dgain;
    std::vector<double> tqpgain, old_tqpgain, default_tqpgain;
    std::vector<double> tqdgain, old_tqdgain, default_tqdgain;

    int m_lLegForceSensorId, m_rLegForceSensorId;
    std::map<std::string, std::vector<int> > m_jointGroups;
    bool m_calibRequested;
    std::string m_calibJointName, m_calibOptions;
    std::string m_pdgainsFilename;
    bool m_reportedEmergency;
    sem_t wait_sem;
    double m_dt;
    std::vector<double> m_commandOld, m_velocityOld;
    hrp::Vector3 G;
    bool m_enable_poweroff_check;
};

#endif