Go to the documentation of this file.00001 #include "RobotHardwareService_impl.h"
00002 #include "robot.h"
00003 #include <hrpModel/Sensor.h>
00004
00005 using namespace OpenHRP;
00006 using namespace hrp;
00007
00008 RobotHardwareService_impl::RobotHardwareService_impl() : m_robot(boost::shared_ptr<robot>())
00009 {
00010 }
00011
00012 RobotHardwareService_impl::~RobotHardwareService_impl()
00013 {
00014 }
00015
00016 #define GetStatus \
00017 \
00018 rs->angle.length(m_robot->numJoints()); \
00019 m_robot->readJointAngles(rs->angle.get_buffer()); \
00020 \
00021 rs->command.length(m_robot->numJoints()); \
00022 m_robot->readJointCommands(rs->command.get_buffer()); \
00023 \
00024 rs->torque.length(m_robot->numJoints()); \
00025 if (!m_robot->readJointTorques(rs->torque.get_buffer())){ \
00026 for (unsigned int i=0; i<rs->torque.length(); i++){ \
00027 rs->torque[i] = 0.0; \
00028 } \
00029 } \
00030 \
00031 rs->servoState.length(m_robot->numJoints()); \
00032 int v, status; \
00033 for(unsigned int i=0; i < rs->servoState.length(); ++i){ \
00034 size_t len = m_robot->lengthOfExtraServoState(i)+1; \
00035 rs->servoState[i].length(len); \
00036 status = 0; \
00037 v = m_robot->readCalibState(i); \
00038 status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; \
00039 v = m_robot->readPowerState(i); \
00040 status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT; \
00041 v = m_robot->readServoState(i); \
00042 status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; \
00043 v = m_robot->readServoAlarm(i); \
00044 status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT; \
00045 v = m_robot->readDriverTemperature(i); \
00046 status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT; \
00047 rs->servoState[i][0] = status; \
00048 m_robot->readExtraServoState(i, (int *)(rs->servoState[i].get_buffer()+1)); \
00049 } \
00050 \
00051 rs->rateGyro.length(m_robot->numSensors(Sensor::RATE_GYRO)); \
00052 for (unsigned int i=0; i<rs->rateGyro.length(); i++){ \
00053 rs->rateGyro[i].length(3); \
00054 m_robot->readGyroSensor(i, rs->rateGyro[i].get_buffer()); \
00055 } \
00056 \
00057 rs->accel.length(m_robot->numSensors(Sensor::ACCELERATION)); \
00058 for (unsigned int i=0; i<rs->accel.length(); i++){ \
00059 rs->accel[i].length(3); \
00060 m_robot->readAccelerometer(i, rs->accel[i].get_buffer()); \
00061 } \
00062 \
00063 rs->force.length(m_robot->numSensors(Sensor::FORCE)); \
00064 for (unsigned int i=0; i<rs->force.length(); i++){ \
00065 rs->force[i].length(6); \
00066 m_robot->readForceSensor(i, rs->force[i].get_buffer()); \
00067 } \
00068 \
00069 m_robot->readPowerStatus(rs->voltage, rs->current);
00070
00071 void RobotHardwareService_impl::getStatus(OpenHRP::RobotHardwareService::RobotState_out rs)
00072 {
00073 rs = new OpenHRP::RobotHardwareService::RobotState();
00074
00075 GetStatus;
00076 }
00077
00078 void RobotHardwareService_impl::getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs)
00079 {
00080 rs = new OpenHRP::RobotHardwareService::RobotState2();
00081
00082 GetStatus;
00083
00084 #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2
00085 rs->batteries.length(m_robot->numBatteries());
00086 for(unsigned int i=0; i<rs->batteries.length(); i++){
00087 m_robot->readBatteryState(i,
00088 rs->batteries[i].voltage,
00089 rs->batteries[i].current,
00090 rs->batteries[i].soc);
00091 }
00092 rs->temperature.length(m_robot->numThermometers());
00093 for (unsigned int i=0; i<rs->temperature.length(); i++){
00094 m_robot->readThermometer(i, rs->temperature[i]);
00095 }
00096 #endif
00097 }
00098
00099 CORBA::Boolean RobotHardwareService_impl::power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss)
00100 {
00101 return m_robot->power(jname, ss==OpenHRP::RobotHardwareService::SWITCH_ON);
00102 }
00103
00104 CORBA::Boolean RobotHardwareService_impl::servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss)
00105 {
00106 return m_robot->servo(jname, ss==OpenHRP::RobotHardwareService::SWITCH_ON);
00107 }
00108
00109 void RobotHardwareService_impl::calibrateInertiaSensor()
00110 {
00111 m_robot->startInertiaSensorCalibration();
00112 }
00113
00114 void RobotHardwareService_impl::removeForceSensorOffset()
00115 {
00116 m_robot->removeForceSensorOffset();
00117 }
00118
00119 void RobotHardwareService_impl::initializeJointAngle(const char* name, const char* option)
00120 {
00121 m_robot->initializeJointAngle(name, option);
00122 }
00123
00124 void RobotHardwareService_impl::setServoGainPercentage(const char *jname, double percentage)
00125 {
00126 m_robot->setServoGainPercentage(jname, percentage);
00127 }
00128
00129 void RobotHardwareService_impl::setServoTorqueGainPercentage(const char *jname, double percentage)
00130 {
00131 m_robot->setServoTorqueGainPercentage(jname, percentage);
00132 }
00133
00134 void RobotHardwareService_impl::setServoErrorLimit(const char *jname, double limit)
00135 {
00136 m_robot->setServoErrorLimit(jname, limit);
00137 }
00138
00139 CORBA::Boolean RobotHardwareService_impl::addJointGroup(const char* gname, const OpenHRP::RobotHardwareService::StrSequence& jnames)
00140 {
00141 std::vector<std::string> joints;
00142 joints.resize(jnames.length());
00143 for (unsigned int i=0; i<jnames.length(); i++){
00144 joints[i] = jnames[i];
00145 }
00146 return m_robot->addJointGroup(gname, joints);
00147 }
00148
00149 CORBA::Boolean RobotHardwareService_impl::readDigitalInput(::OpenHRP::RobotHardwareService::OctSequence_out din)
00150 {
00151 din = new ::OpenHRP::RobotHardwareService::OctSequence();
00152 din->length(lengthDigitalInput());
00153 return m_robot->readDigitalInput((char *)(din->get_buffer()));
00154 }
00155
00156 CORBA::Long RobotHardwareService_impl::lengthDigitalInput()
00157 {
00158 return m_robot->lengthDigitalInput();
00159 }
00160
00161 CORBA::Boolean RobotHardwareService_impl::writeDigitalOutput(const ::OpenHRP::RobotHardwareService::OctSequence& dout)
00162 {
00163 return m_robot->writeDigitalOutput((const char *)(dout.get_buffer()));
00164 }
00165
00166 CORBA::Boolean RobotHardwareService_impl::writeDigitalOutputWithMask(const ::OpenHRP::RobotHardwareService::OctSequence& dout, const ::OpenHRP::RobotHardwareService::OctSequence& mask)
00167 {
00168 return m_robot->writeDigitalOutputWithMask((const char *)(dout.get_buffer()), (const char *)(mask.get_buffer()));
00169 }
00170
00171 CORBA::Long RobotHardwareService_impl::lengthDigitalOutput()
00172 {
00173 return m_robot->lengthDigitalOutput();
00174 }
00175
00176 CORBA::Boolean RobotHardwareService_impl::readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout)
00177 {
00178 dout = new ::OpenHRP::RobotHardwareService::OctSequence();
00179 dout->length(lengthDigitalOutput());
00180 return m_robot->readDigitalOutput((char *)(dout->get_buffer()));
00181 }
00182
00183 CORBA::Boolean RobotHardwareService_impl::setJointInertia(const char* name, ::CORBA::Double mn)
00184 {
00185 m_robot->setJointInertia(name, mn);
00186 }
00187
00188 void RobotHardwareService_impl::setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns)
00189 {
00190 m_robot->setJointInertias(mns.get_buffer());
00191 }
00192
00193
00194 void RobotHardwareService_impl::enableDisturbanceObserver()
00195 {
00196 m_robot->enableDisturbanceObserver();
00197 }
00198
00199 void RobotHardwareService_impl::disableDisturbanceObserver()
00200 {
00201 m_robot->disableDisturbanceObserver();
00202 }
00203
00204 void RobotHardwareService_impl::setDisturbanceObserverGain(::CORBA::Double gain)
00205 {
00206 m_robot->setDisturbanceObserverGain(gain);
00207 }
00208
00209 void RobotHardwareService_impl::setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm)
00210 {
00211 joint_control_mode mode;
00212 switch(jcm){
00213 case OpenHRP::RobotHardwareService::FREE:
00214 mode = JCM_FREE;
00215 break;
00216 case OpenHRP::RobotHardwareService::POSITION:
00217 mode = JCM_POSITION;
00218 break;
00219 case OpenHRP::RobotHardwareService::TORQUE:
00220 mode = JCM_TORQUE;
00221 break;
00222 case OpenHRP::RobotHardwareService::VELOCITY:
00223 mode = JCM_VELOCITY;
00224 break;
00225 #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4
00226 case OpenHRP::RobotHardwareService::POSITION_TORQUE:
00227 mode = JCM_POSITION_TORQUE;
00228 break;
00229 #endif
00230 default:
00231 return;
00232 }
00233 m_robot->setJointControlMode(jname, mode);
00234 }