dynamixel_io.cpp
Go to the documentation of this file.
00001 #include <dynamixel_io.h>
00002 #include <dynamixel_const.h>
00003 #include <boost/python.hpp>
00004 #include <boost/python/def.hpp>
00005 
00006 using namespace dynamixel_hardware_interface;
00007 using namespace boost::python;
00008 
00009 template <class T>
00010 std::vector<std::vector<T> > nestedSequenceToNestedVector(object t)
00011 {
00012     std::vector<std::vector<T> > result;
00013     
00014     for (int i = 0; i < len(t); ++i)
00015     {
00016         std::vector<T> v;
00017         object mvals = t[i];
00018         
00019         for (int j = 0; j < len(mvals); ++j)
00020         {
00021             v.push_back(extract<T>(mvals[j]));
00022         }
00023         
00024         result.push_back(v);
00025     }
00026     
00027     return result;
00028 }
00029 
00030 std::vector<uint8_t> sequenceToVector(object l)
00031 {
00032     std::vector<uint8_t> result;
00033     
00034     for (int i = 0; i < len(l); ++i)
00035     {
00036         result.push_back(extract<uint8_t>(l[i]));
00037     }
00038     
00039     return result;
00040 }
00041 
00042 list vectorToPyList(const std::vector<uint8_t>& vec)
00043 {
00044     list result;
00045     
00046     for (int i = 0; i < vec.size(); ++i)
00047     {
00048         result.append(vec[i]);
00049     }
00050     
00051     return result;
00052 }
00053 
00054 class DynamixelIOWrap : public DynamixelIO
00055 {
00056 public:
00057     DynamixelIOWrap(std::string device, int baud) : DynamixelIO(device, boost::lexical_cast<std::string>(baud)) {}
00058     
00059     object getModelNumber(int servo_id)
00060     {
00061         uint16_t model;
00062         if (!DynamixelIO::getModelNumber(servo_id, model)) { return object(); }
00063         else { return object(model); }
00064     }
00065     
00066     object getFirmwareVersion(int servo_id)
00067     {
00068         uint8_t fw_ver;
00069         if (!DynamixelIO::getFirmwareVersion(servo_id, fw_ver)) { return object(); }
00070         else { return object(fw_ver); }
00071     }
00072 
00073     object getBaudRate(int servo_id)
00074     {
00075         uint8_t baud_rate;
00076         if (!DynamixelIO::getBaudRate(servo_id, baud_rate)) { return object(); }
00077         else { return object(baud_rate); }
00078     }
00079     
00080     object getReturnDelayTime(int servo_id)
00081     {
00082         uint8_t return_delay_time;
00083         if (!DynamixelIO::getReturnDelayTime(servo_id, return_delay_time)) { return object(); }
00084         else { return object(return_delay_time); }
00085     }
00086     
00087     object getAngleLimits(int servo_id)
00088     {
00089         uint16_t cw_angle_limit;
00090         uint16_t ccw_angle_limit;
00091         if (!DynamixelIO::getAngleLimits(servo_id, cw_angle_limit, ccw_angle_limit)) { return object(); }
00092         else { return make_tuple(cw_angle_limit, ccw_angle_limit); }
00093     }
00094     
00095     object getCWAngleLimit(int servo_id)
00096     {
00097         uint16_t cw_angle_limit;
00098         if (!DynamixelIO::getCWAngleLimit(servo_id, cw_angle_limit)) { return object(); }
00099         else { return object(cw_angle_limit); }
00100     }
00101     
00102     object getCCWAngleLimit(int servo_id)
00103     {
00104         uint16_t ccw_angle_limit;
00105         if (!DynamixelIO::getCCWAngleLimit(servo_id, ccw_angle_limit)) { return object(); }
00106         else { return object(ccw_angle_limit); }
00107     }
00108     
00109     object getVoltageLimits(int servo_id)
00110     {
00111         float min_voltage_limit;
00112         float max_voltage_limit;
00113         if (!DynamixelIO::getVoltageLimits(servo_id, min_voltage_limit, max_voltage_limit)) { return object(); }
00114         else { return make_tuple(min_voltage_limit, max_voltage_limit); }
00115     }
00116     
00117     object getMinVoltageLimit(int servo_id)
00118     {
00119         float min_voltage_limit;
00120         if (!DynamixelIO::getMinVoltageLimit(servo_id, min_voltage_limit)) { return object(); }
00121         else { return object(min_voltage_limit); }
00122     }
00123     
00124     object getMaxVoltageLimit(int servo_id)
00125     {
00126         float max_voltage_limit;
00127         if (!DynamixelIO::getMaxVoltageLimit(servo_id, max_voltage_limit)) { return object(); }
00128         else { return object(max_voltage_limit); }
00129     }
00130     
00131     object getTemperatureLimit(int servo_id)
00132     {
00133         uint8_t max_temperature;
00134         if (!DynamixelIO::getTemperatureLimit(servo_id, max_temperature)) { return object(); }
00135         else { return object(max_temperature); }
00136     }
00137     
00138     object getMaxTorque(int servo_id)
00139     {
00140         uint16_t max_torque;
00141         if (!DynamixelIO::getMaxTorque(servo_id, max_torque)) { return object(); }
00142         else { return object(max_torque); }
00143     }
00144     
00145     object getAlarmLed(int servo_id)
00146     {
00147         uint8_t alarm_led;
00148         if (!DynamixelIO::getAlarmLed(servo_id, alarm_led)) { return object(); }
00149         else { return object(alarm_led); }
00150     }
00151     
00152     object getAlarmShutdown(int servo_id)
00153     {
00154         uint8_t alarm_shutdown;
00155         if (!DynamixelIO::getAlarmShutdown(servo_id, alarm_shutdown)) { return object(); }
00156         else { return object(alarm_shutdown); }
00157     }
00158 
00159     object getTorqueEnable(int servo_id)
00160     {
00161         bool torque_enabled;
00162         if (!DynamixelIO::getTorqueEnable(servo_id, torque_enabled)) { return object(); }
00163         else { return object(torque_enabled); }
00164     }
00165 
00166     object getLedStatus(int servo_id)
00167     {
00168         bool led_enabled;
00169         if (!DynamixelIO::getLedStatus(servo_id, led_enabled)) { return object(); }
00170         else { return object(led_enabled); }
00171     }
00172 
00173     object getComplianceMargins(int servo_id)
00174     {
00175         uint8_t cw_compliance_margin;
00176         uint8_t ccw_compliance_margin;
00177         if (!DynamixelIO::getComplianceMargins(servo_id, cw_compliance_margin, ccw_compliance_margin)) { return object(); }
00178         else { return make_tuple(cw_compliance_margin, ccw_compliance_margin); }
00179     }
00180     
00181     object getCWComplianceMargin(int servo_id)
00182     {
00183         uint8_t cw_compliance_margin;
00184         if (!DynamixelIO::getCWComplianceMargin(servo_id, cw_compliance_margin)) { return object(); }
00185         else { return object(cw_compliance_margin); }
00186     }
00187 
00188     object getCCWComplianceMargin(int servo_id)
00189     {
00190         uint8_t ccw_compliance_margin;
00191         if (!DynamixelIO::getCCWComplianceMargin(servo_id, ccw_compliance_margin)) { return object(); }
00192         else { return object(ccw_compliance_margin); }
00193     }
00194 
00195     object getComplianceSlopes(int servo_id)
00196     {
00197         uint8_t cw_compliance_slope;
00198         uint8_t ccw_compliance_slope;
00199         if (!DynamixelIO::getComplianceSlopes(servo_id, cw_compliance_slope, ccw_compliance_slope)) { return object(); }
00200         else { return make_tuple(cw_compliance_slope, ccw_compliance_slope); }
00201     }
00202     
00203     object getCWComplianceSlope(int servo_id)
00204     {
00205         uint8_t cw_compliance_slope;
00206         if (!DynamixelIO::getCWComplianceSlope(servo_id, cw_compliance_slope)) { return object(); }
00207         else { return object(cw_compliance_slope); }
00208     }
00209 
00210     object getCCWComplianceSlope(int servo_id)
00211     {
00212         uint8_t ccw_compliance_slope;
00213         if (!DynamixelIO::getCCWComplianceSlope(servo_id, ccw_compliance_slope)) { return object(); }
00214         else { return object(ccw_compliance_slope); }
00215     }
00216 
00217     object getTargetPosition(int servo_id)
00218     {
00219         uint16_t target_position;
00220         if (!DynamixelIO::getTargetPosition(servo_id, target_position)) { return object(); }
00221         else { return object(target_position); }
00222     }
00223 
00224     object getTargetVelocity(int servo_id)
00225     {
00226         int16_t target_velocity;
00227         if (!DynamixelIO::getTargetVelocity(servo_id, target_velocity)) { return object(); }
00228         else { return object(target_velocity); }
00229     }
00230 
00231     object getTorqueLimit(int servo_id)
00232     {
00233         uint16_t target_limit;
00234         if (!DynamixelIO::getTorqueLimit(servo_id, target_limit)) { return object(); }
00235         else { return object(target_limit); }
00236     }
00237 
00238     object getPosition(int servo_id)
00239     {
00240         uint16_t position;
00241         if (!DynamixelIO::getPosition(servo_id, position)) { return object(); }
00242         else { return object(position); }
00243     }
00244 
00245     object getVelocity(int servo_id)
00246     {
00247         int16_t velocity;
00248         if (!DynamixelIO::getVelocity(servo_id, velocity)) { return object(); }
00249         else { return object(velocity); }
00250     }
00251 
00252     object getLoad(int servo_id)
00253     {
00254         int16_t load;
00255         if (!DynamixelIO::getLoad(servo_id, load)) { return object(); }
00256         else { return object(load); }
00257     }
00258 
00259     object getVoltage(int servo_id)
00260     {
00261         float voltage;
00262         if (!DynamixelIO::getVoltage(servo_id, voltage)) { return object(); }
00263         else { return object(voltage); }
00264     }
00265 
00266     object getTemperature(int servo_id)
00267     {
00268         uint8_t temperature;
00269         if (!DynamixelIO::getTemperature(servo_id, temperature)) { return object(); }
00270         else { return object(temperature); }
00271     }
00272 
00273     object getMoving(int servo_id)
00274     {
00275         bool is_moving;
00276         if (!DynamixelIO::getMoving(servo_id, is_moving)) { return object(); }
00277         else { return object(is_moving); }
00278     }
00279 
00280     object getFeedback(int servo_id)
00281     {
00282         DynamixelStatus status;
00283         
00284         if (!DynamixelIO::getFeedback(servo_id, status))
00285         {
00286             return object();
00287         }
00288         else
00289         {
00290             dict status_dict;
00291             
00292             status_dict["timestamp"] = status.timestamp;
00293             status_dict["torque_limit"] = status.torque_limit;
00294             status_dict["position"] = status.position;
00295             status_dict["velocity"] = status.velocity;
00296             status_dict["load"] = status.load;
00297             status_dict["voltage"] = status.voltage;
00298             status_dict["temperature"] = status.temperature;
00299             status_dict["moving"] = status.moving;
00300             
00301             return status_dict;
00302         }
00303     }
00304     
00305     bool setMultiPosition(object value_pairs)
00306     {
00307         return DynamixelIO::setMultiPosition(nestedSequenceToNestedVector<int>(value_pairs));
00308     }
00309 
00310     bool setMultiVelocity(object value_pairs)
00311     {
00312         return DynamixelIO::setMultiVelocity(nestedSequenceToNestedVector<int>(value_pairs));
00313     }
00314 
00315     bool setMultiPositionVelocity(object value_pairs)
00316     {
00317         return DynamixelIO::setMultiPositionVelocity(nestedSequenceToNestedVector<int>(value_pairs));
00318     }
00319 
00320     bool setMultiComplianceMargins(object value_pairs)
00321     {
00322         return DynamixelIO::setMultiComplianceMargins(nestedSequenceToNestedVector<int>(value_pairs));
00323     }
00324     
00325     bool setMultiComplianceSlopes(object value_pairs)
00326     {
00327         return DynamixelIO::setMultiComplianceSlopes(nestedSequenceToNestedVector<int>(value_pairs));
00328     }
00329     
00330     bool setMultiTorqueEnabled(object value_pairs)
00331     {
00332         return DynamixelIO::setMultiTorqueEnabled(nestedSequenceToNestedVector<int>(value_pairs));
00333     }
00334     
00335     bool setMultiTorqueLimit(object value_pairs)
00336     {
00337         return DynamixelIO::setMultiTorqueLimit(nestedSequenceToNestedVector<int>(value_pairs));
00338     }
00339     
00340     object read(int servo_id, DynamixelControl address, int size)
00341     {
00342         std::vector<uint8_t> response;
00343         if (!DynamixelIO::read(servo_id, address, size, response)) { return object(); }
00344         else { return vectorToPyList(response); }
00345     }
00346 
00347     object write(int servo_id, DynamixelControl address, object data)
00348     {
00349         std::vector<uint8_t> response;
00350         std::vector<uint8_t> datavec = sequenceToVector(data);
00351         
00352         if (!DynamixelIO::write(servo_id, address, datavec, response)) { return object(); }
00353         else { return vectorToPyList(response); }
00354     }
00355 
00356     bool syncWrite(DynamixelControl address, object data)
00357     {
00358         std::vector<std::vector<uint8_t> > datavec = nestedSequenceToNestedVector<uint8_t>(data);
00359         return DynamixelIO::syncWrite(address, datavec);
00360     }
00361 };
00362 
00363 
00364 
00365 BOOST_PYTHON_MODULE(dynamixel_io)
00366 {
00367     // This will enable user-defined docstrings and python signatures,
00368     // while disabling the C++ signatures
00369     docstring_options local_docstring_options(true, true, false);
00370 
00371     def("get_motor_model_name", &getMotorModelName);
00372     def("get_motor_model_params", &getMotorModelParams);
00373     
00374     scope().attr("DXL_MAX_LOAD_ENCODER") = DXL_MAX_LOAD_ENCODER;
00375     scope().attr("DXL_MAX_VELOCITY_ENCODER") = DXL_MAX_VELOCITY_ENCODER;
00376     scope().attr("DXL_MAX_TORQUE_ENCODER") = DXL_MAX_TORQUE_ENCODER;
00377 
00378     enum_<DynamixelControl>("DynamixelControl")
00379         .value("DXL_MODEL_NUMBER_L", DXL_MODEL_NUMBER_L)
00380         .value("DXL_MODEL_NUMBER_H", DXL_MODEL_NUMBER_H)
00381         .value("DXL_FIRMWARE_VERSION", DXL_FIRMWARE_VERSION)
00382         .value("DXL_ID", DXL_ID)
00383         .value("DXL_BAUD_RATE", DXL_BAUD_RATE)
00384         .value("DXL_RETURN_DELAY_TIME", DXL_RETURN_DELAY_TIME)
00385         .value("DXL_CW_ANGLE_LIMIT_L", DXL_CW_ANGLE_LIMIT_L)
00386         .value("DXL_CW_ANGLE_LIMIT_H", DXL_CW_ANGLE_LIMIT_H)
00387         .value("DXL_CCW_ANGLE_LIMIT_L", DXL_CCW_ANGLE_LIMIT_L)
00388         .value("DXL_CCW_ANGLE_LIMIT_H", DXL_CCW_ANGLE_LIMIT_H)
00389         .value("DXL_DRIVE_MODE", DXL_DRIVE_MODE)
00390         .value("DXL_LIMIT_TEMPERATURE", DXL_LIMIT_TEMPERATURE)
00391         .value("DXL_DOWN_LIMIT_VOLTAGE", DXL_DOWN_LIMIT_VOLTAGE)
00392         .value("DXL_UP_LIMIT_VOLTAGE", DXL_UP_LIMIT_VOLTAGE)
00393         .value("DXL_MAX_TORQUE_L", DXL_MAX_TORQUE_L)
00394         .value("DXL_MAX_TORQUE_H", DXL_MAX_TORQUE_H)
00395         .value("DXL_RETURN_LEVEL", DXL_RETURN_LEVEL)
00396         .value("DXL_ALARM_LED", DXL_ALARM_LED)
00397         .value("DXL_ALARM_SHUTDOWN", DXL_ALARM_SHUTDOWN)
00398         .value("DXL_OPERATING_MODE", DXL_OPERATING_MODE)
00399         .value("DXL_DOWN_CALIBRATION_L", DXL_DOWN_CALIBRATION_L)
00400         .value("DXL_DOWN_CALIBRATION_H", DXL_DOWN_CALIBRATION_H)
00401         .value("DXL_UP_CALIBRATION_L", DXL_UP_CALIBRATION_L)
00402         .value("DXL_UP_CALIBRATION_H", DXL_UP_CALIBRATION_H)
00403         .value("DXL_TORQUE_ENABLE", DXL_TORQUE_ENABLE)
00404         .value("DXL_LED", DXL_LED)
00405         .value("DXL_CW_COMPLIANCE_MARGIN", DXL_CW_COMPLIANCE_MARGIN)
00406         .value("DXL_CCW_COMPLIANCE_MARGIN", DXL_CCW_COMPLIANCE_MARGIN)
00407         .value("DXL_CW_COMPLIANCE_SLOPE", DXL_CW_COMPLIANCE_SLOPE)
00408         .value("DXL_CCW_COMPLIANCE_SLOPE", DXL_CCW_COMPLIANCE_SLOPE)
00409         .value("DXL_GOAL_POSITION_L", DXL_GOAL_POSITION_L)
00410         .value("DXL_GOAL_POSITION_H", DXL_GOAL_POSITION_H)
00411         .value("DXL_GOAL_SPEED_L", DXL_GOAL_SPEED_L)
00412         .value("DXL_GOAL_SPEED_H", DXL_GOAL_SPEED_H)
00413         .value("DXL_TORQUE_LIMIT_L", DXL_TORQUE_LIMIT_L)
00414         .value("DXL_TORQUE_LIMIT_H", DXL_TORQUE_LIMIT_H)
00415         .value("DXL_PRESENT_POSITION_L", DXL_PRESENT_POSITION_L)
00416         .value("DXL_PRESENT_POSITION_H", DXL_PRESENT_POSITION_H)
00417         .value("DXL_PRESENT_SPEED_L", DXL_PRESENT_SPEED_L)
00418         .value("DXL_PRESENT_SPEED_H", DXL_PRESENT_SPEED_H)
00419         .value("DXL_PRESENT_LOAD_L", DXL_PRESENT_LOAD_L)
00420         .value("DXL_PRESENT_LOAD_H", DXL_PRESENT_LOAD_H)
00421         .value("DXL_PRESENT_VOLTAGE", DXL_PRESENT_VOLTAGE)
00422         .value("DXL_PRESENT_TEMPERATURE", DXL_PRESENT_TEMPERATURE)
00423         .value("DXL_REGISTERED_INSTRUCTION", DXL_REGISTERED_INSTRUCTION)
00424         .value("DXL_PAUSE_TIME", DXL_PAUSE_TIME)
00425         .value("DXL_MOVING", DXL_MOVING)
00426         .value("DXL_LOCK", DXL_LOCK)
00427         .value("DXL_PUNCH_L", DXL_PUNCH_L)
00428         .value("DXL_PUNCH_H", DXL_PUNCH_H)
00429         .value("DXL_SENSED_CURRENT_L", DXL_SENSED_CURRENT_L)
00430         .value("DXL_SENSED_CURRENT_H", DXL_SENSED_CURRENT_H)
00431         ;
00432     
00433     enum_<DynamixelInstruction>("DynamixelInstruction")
00434         .value("DXL_PING", DXL_PING)
00435         .value("DXL_READ_DATA", DXL_READ_DATA)
00436         .value("DXL_WRITE_DATA", DXL_WRITE_DATA)
00437         .value("DXL_REG_WRITE", DXL_REG_WRITE)
00438         .value("DXL_ACTION", DXL_ACTION)
00439         .value("DXL_RESET", DXL_RESET)
00440         .value("DXL_SYNC_WRITE", DXL_SYNC_WRITE)
00441         .value("DXL_BROADCAST", DXL_BROADCAST)
00442         ;
00443     
00444     enum_<DynamixelErrorCode>("DynamixelErrorCode")
00445         .value("DXL_INSTRUCTION_ERROR", DXL_INSTRUCTION_ERROR)
00446         .value("DXL_OVERLOAD_ERROR", DXL_OVERLOAD_ERROR)
00447         .value("DXL_CHECKSUM_ERROR", DXL_CHECKSUM_ERROR)
00448         .value("DXL_RANGE_ERROR", DXL_RANGE_ERROR)
00449         .value("DXL_OVERHEATING_ERROR", DXL_OVERHEATING_ERROR)
00450         .value("DXL_ANGLE_LIMIT_ERROR", DXL_ANGLE_LIMIT_ERROR)
00451         .value("DXL_INPUT_VOLTAGE_ERROR", DXL_INPUT_VOLTAGE_ERROR)
00452         .value("DXL_NO_ERROR", DXL_NO_ERROR)
00453         ;
00454 
00455     enum_<DynamixelParams>("DynamixelParams")
00456         .value("ENCODER_RESOLUTION", ENCODER_RESOLUTION)
00457         .value("RANGE_DEGREES", RANGE_DEGREES)
00458         .value("TORQUE_PER_VOLT", TORQUE_PER_VOLT)
00459         .value("VELOCITY_PER_VOLT", VELOCITY_PER_VOLT)
00460         ;
00461     
00462     class_<DynamixelData> ("DynamixelData")
00463         .def_readwrite("model_number", &DynamixelData::model_number)
00464         .def_readwrite("firmware_version", &DynamixelData::firmware_version)
00465         .def_readwrite("id", &DynamixelData::id)
00466         .def_readwrite("baud_rate", &DynamixelData::baud_rate)
00467         .def_readwrite("return_delay_time", &DynamixelData::return_delay_time)
00468         .def_readwrite("cw_angle_limit", &DynamixelData::cw_angle_limit)
00469         .def_readwrite("ccw_angle_limit", &DynamixelData::ccw_angle_limit)
00470         .def_readwrite("drive_mode", &DynamixelData::drive_mode)
00471         .def_readwrite("temperature_limit", &DynamixelData::temperature_limit)
00472         .def_readwrite("voltage_limit_low", &DynamixelData::voltage_limit_low)
00473         .def_readwrite("voltage_limit_high", &DynamixelData::voltage_limit_high)
00474         .def_readwrite("max_torque", &DynamixelData::max_torque)
00475         .def_readwrite("return_level", &DynamixelData::return_level)
00476         .def_readwrite("alarm_led", &DynamixelData::alarm_led)
00477         .def_readwrite("alarm_shutdown", &DynamixelData::alarm_shutdown)
00478         .def_readwrite("torque_enabled", &DynamixelData::torque_enabled)
00479         .def_readwrite("led", &DynamixelData::led)
00480         .def_readwrite("cw_compliance_margin", &DynamixelData::cw_compliance_margin)
00481         .def_readwrite("ccw_compliance_margin", &DynamixelData::ccw_compliance_margin)
00482         .def_readwrite("cw_compliance_slope", &DynamixelData::cw_compliance_slope)
00483         .def_readwrite("ccw_compliance_slope", &DynamixelData::ccw_compliance_slope)
00484         .def_readwrite("target_position", &DynamixelData::target_position)
00485         .def_readwrite("target_velocity", &DynamixelData::target_velocity)
00486         .def_readwrite("shutdown_error_time", &DynamixelData::shutdown_error_time)
00487         .def_readwrite("error", &DynamixelData::error)
00488         ;
00489     
00490     class_<DynamixelStatus> ("DynamixelStatus")
00491         .def_readwrite("timestamp", &DynamixelStatus::timestamp)
00492         .def_readwrite("torque_limit", &DynamixelStatus::torque_limit)
00493         .def_readwrite("position", &DynamixelStatus::position)
00494         .def_readwrite("velocity", &DynamixelStatus::velocity)
00495         .def_readwrite("load", &DynamixelStatus::load)
00496         .def_readwrite("voltage", &DynamixelStatus::voltage)
00497         .def_readwrite("temperature", &DynamixelStatus::temperature)
00498         .def_readwrite("moving", &DynamixelStatus::moving)
00499         ;
00500     
00501     class_<DynamixelIOWrap, boost::noncopyable> ("DynamixelIO", init<std::string, int> ())
00502         .def("get_cached_parameters", &DynamixelIOWrap::getCachedParameters, return_value_policy<manage_new_object>())
00503         .def ("ping", &DynamixelIOWrap::ping,
00504               "Returns True if specified servo_id is connected and responding properly, False otherwise.\n\n"
00505               "Usage:\n"
00506               ">>> dxl_io.ping(4)\n"
00507               "True\n"
00508               ">>> dxl_io.ping(20)\n"
00509               "False")
00510         .def ("reset_overload_error", &DynamixelIOWrap::resetOverloadError)
00511         
00512         // ****************************** GETTERS ******************************** //
00513         .def ("get_model_number", &DynamixelIOWrap::getModelNumber)
00514         .def ("get_firmware_version", &DynamixelIOWrap::getFirmwareVersion)
00515         .def ("get_baud_rate", &DynamixelIOWrap::getBaudRate)
00516         .def ("get_return_delay_time", &DynamixelIOWrap::getReturnDelayTime)
00517         
00518         .def ("get_angle_limits", &DynamixelIOWrap::getAngleLimits)
00519         .def ("get_cw_angle_limit", &DynamixelIOWrap::getCWAngleLimit)
00520         .def ("get_ccw_angle_limit", &DynamixelIOWrap::getCCWAngleLimit)
00521         
00522         .def ("get_voltage_limits", &DynamixelIOWrap::getVoltageLimits)
00523         .def ("get_min_voltage_limit", &DynamixelIOWrap::getMinVoltageLimit)
00524         .def ("get_max_voltage_limit", &DynamixelIOWrap::getMaxVoltageLimit)
00525         
00526         .def ("get_temperature_limit", &DynamixelIOWrap::getTemperatureLimit)
00527         .def ("get_max_torque", &DynamixelIOWrap::getMaxTorque)
00528         .def ("get_alarm_led", &DynamixelIOWrap::getAlarmLed)
00529         .def ("get_alarm_shutdown", &DynamixelIOWrap::getAlarmShutdown)
00530         .def ("get_torque_enable", &DynamixelIOWrap::getTorqueEnable)
00531         .def ("get_led_status", &DynamixelIOWrap::getLedStatus)
00532         
00533         .def ("get_compliance_margins", &DynamixelIOWrap::getComplianceMargins)
00534         .def ("get_cw_compliance_margin", &DynamixelIOWrap::getCWComplianceMargin)
00535         .def ("get_ccw_compliance_margin", &DynamixelIOWrap::getCCWComplianceMargin)
00536         
00537         .def ("get_compliance_slopes", &DynamixelIOWrap::getComplianceSlopes)
00538         .def ("get_cw_compliance_slope", &DynamixelIOWrap::getCWComplianceSlope)
00539         .def ("get_ccw_compliance_slope", &DynamixelIOWrap::getCCWComplianceSlope)
00540         
00541         .def ("get_target_position", &DynamixelIOWrap::getTargetPosition)
00542         .def ("get_target_velocity", &DynamixelIOWrap::getTargetVelocity)
00543         .def ("get_torque_limit", &DynamixelIOWrap::getTorqueLimit)
00544         
00545         .def ("get_position", &DynamixelIOWrap::getPosition)
00546         .def ("get_velocity", &DynamixelIOWrap::getVelocity)
00547         .def ("get_load", &DynamixelIOWrap::getLoad)
00548         .def ("get_voltage", &DynamixelIOWrap::getVoltage)
00549         .def ("get_temperature", &DynamixelIOWrap::getTemperature)
00550         .def ("get_moving", &DynamixelIOWrap::getMoving)
00551         
00552         .def ("get_feedback", &DynamixelIOWrap::getFeedback)
00553 
00554         // ****************************** SETTERS ******************************** //
00555         .def ("set_id", &DynamixelIOWrap::setId)
00556         .def ("set_baud_rate", &DynamixelIOWrap::setBaudRate)
00557         .def ("set_return_delay_time", &DynamixelIOWrap::setReturnDelayTime)
00558         
00559         .def ("set_angle_limits", &DynamixelIOWrap::setAngleLimits)
00560         .def ("set_cw_angle_limit", &DynamixelIOWrap::setCWAngleLimit)
00561         .def ("set_ccw_angle_limit", &DynamixelIOWrap::setCCWAngleLimit)
00562     
00563         .def ("set_voltage_limits", &DynamixelIOWrap::setVoltageLimits)
00564         .def ("set_min_voltage_limit", &DynamixelIOWrap::setMinVoltageLimit)
00565         .def ("set_max_voltage_limit", &DynamixelIOWrap::setMaxVoltageLimit)
00566     
00567         .def ("set_temperature_limit", &DynamixelIOWrap::setTemperatureLimit)
00568         .def ("set_max_torque", &DynamixelIOWrap::setMaxTorque)
00569         .def ("set_alarm_led", &DynamixelIOWrap::setAlarmLed)
00570         .def ("set_alarm_shutdown", &DynamixelIOWrap::setAlarmShutdown)
00571         .def ("set_torque_enable", &DynamixelIOWrap::setTorqueEnable)
00572         .def ("set_led", &DynamixelIOWrap::setLed)
00573 
00574         .def ("set_compliance_margins", &DynamixelIOWrap::setComplianceMargins)
00575         .def ("set_cw_compliance_margin", &DynamixelIOWrap::setCWComplianceMargin)
00576         .def ("set_ccw_compliance_margin", &DynamixelIOWrap::setCCWComplianceMargin)
00577         
00578         .def ("set_compliance_slopes", &DynamixelIOWrap::setComplianceSlopes)
00579         .def ("set_cw_compliance_slope", &DynamixelIOWrap::setCWComplianceSlope)
00580         .def ("set_ccw_compliance_slope", &DynamixelIOWrap::setCCWComplianceSlope)
00581 
00582         .def ("set_position", &DynamixelIOWrap::setPosition)
00583         .def ("set_velocity", &DynamixelIOWrap::setVelocity)
00584         .def ("set_torque_limit", &DynamixelIOWrap::setTorqueLimit)
00585         
00586         // ************************* SYNC_WRITE METHODS *************************** //
00587         .def ("set_multi_position", &DynamixelIOWrap::setMultiPosition)
00588         .def ("set_multi_velocity", &DynamixelIOWrap::setMultiVelocity)
00589         .def ("set_multi_position_velocity", &DynamixelIOWrap::setMultiPositionVelocity)
00590         .def ("set_multi_compliance_margins", &DynamixelIOWrap::setMultiComplianceMargins)
00591         .def ("set_multi_compliance_slopes", &DynamixelIOWrap::setMultiComplianceSlopes)
00592         .def ("set_multi_torque_enabled", &DynamixelIOWrap::setMultiTorqueEnabled)
00593         .def ("set_multi_torque_limit", &DynamixelIOWrap::setMultiTorqueLimit)
00594 
00595         .def ("read", &DynamixelIOWrap::read)
00596         .def ("write", &DynamixelIOWrap::write)
00597         .def ("sync_write", &DynamixelIOWrap::syncWrite)
00598         ;
00599 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Fri Aug 28 2015 10:32:45