#include <unistd.h>#include <iostream>#include <cstdlib>#include <cstring>#include <vector>#include "hrpsys/io/iob.h"#include <ros/ros.h>#include <boost/algorithm/string.hpp>#include <hrpsys_gazebo_msgs/JointCommand.h>#include <hrpsys_gazebo_msgs/RobotState.h>#include <hrpUtil/Eigen3d.h>
Go to the source code of this file.
Defines | |
| #define | CHECK_ACCELEROMETER_ID(id) if ((id) < 0 || (id) >= number_of_accelerometers()) return E_ID |
| #define | CHECK_ATTITUDE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID |
| #define | CHECK_FORCE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_force_sensors()) return E_ID |
| #define | CHECK_GYRO_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_gyro_sensors()) return E_ID |
| #define | CHECK_JOINT_ID(id) if ((id) < 0 || (id) >= number_of_joints()) return E_ID |
| #define | JOINT_ID_MODEL2REAL(id) joint_id_model2real(id) |
| #define | JOINT_ID_REAL2MODEL(id) joint_real2model_vec[id] |
| #define | NUM_OF_REAL_JOINT (joint_real2model_vec.size()) |
Typedefs | |
| typedef hrpsys_gazebo_msgs::JointCommand | JointCommand |
| typedef hrpsys_gazebo_msgs::RobotState | RobotState |
Functions | |
| int | close_iob (void) |
| long | get_signal_period () |
| int | initializeJointAngle (const char *name, const char *option) |
| int | joint_calibration (int id, double angle) |
| static int | joint_id_model2real (int id) |
| int | length_digital_input (void) |
| int | length_digital_output (void) |
| size_t | length_of_extra_servo_state (int id) |
| int | lock_iob () |
| int | number_of_accelerometers () |
| int | number_of_attitude_sensors () |
| int | number_of_force_sensors () |
| int | number_of_gyro_sensors () |
| int | number_of_joints () |
| int | number_of_substeps () |
| int | open_iob (void) |
| int | read_accelerometer (int id, double *accels) |
| int | read_accelerometer_offset (int id, double *offset) |
| int | read_actual_angle (int id, double *angle) |
| int | read_actual_angles (double *angles) |
| int | read_actual_torques (double *torques) |
| int | read_angle_offset (int id, double *angle) |
| int | read_attitude_sensor (int id, double *att) |
| int | read_calib_state (int id, int *s) |
| int | read_command_angle (int id, double *angle) |
| int | read_command_angles (double *angles) |
| int | read_command_torque (int id, double *torque) |
| int | read_command_torques (double *torques) |
| int | read_command_velocities (double *vels) |
| int | read_command_velocity (int id, double *vel) |
| int | read_control_mode (int id, joint_control_mode *s) |
| int | read_current (int id, double *mcurrent) |
| int | read_current_limit (int id, double *v) |
| int | read_currents (double *currents) |
| int | read_dgain (int id, double *gain) |
| int | read_digital_input (char *dinput) |
| int | read_driver_temperature (int id, unsigned char *v) |
| int | read_encoder_pulse (int id, double *ec) |
| int | read_extra_servo_state (int id, int *state) |
| int | read_force_offset (int id, double *offsets) |
| int | read_force_sensor (int id, double *forces) |
| int | read_gauges (double *gauges) |
| int | read_gear_ratio (int id, double *gr) |
| int | read_gyro_sensor (int id, double *rates) |
| int | read_gyro_sensor_offset (int id, double *offset) |
| unsigned long long | read_iob_frame () |
| int | read_limit_angle (int id, double *angle) |
| int | read_llimit_angle (int id, double *angle) |
| int | read_lock_owner (pid_t *pid) |
| int | read_pgain (int id, double *gain) |
| int | read_power (double *voltage, double *current) |
| int | read_power_command (int id, int *com) |
| int | read_power_state (int id, int *s) |
| int | read_servo_alarm (int id, int *a) |
| int | read_servo_state (int id, int *s) |
| int | read_temperature (int id, double *v) |
| int | read_torque_const (int id, double *tc) |
| int | read_torque_limit (int id, double *limit) |
| int | read_touch_sensors (unsigned short *onoff) |
| int | read_ulimit_angle (int id, double *angle) |
| int | reset_body (void) |
| int | set_number_of_accelerometers (int num) |
| int | set_number_of_attitude_sensors (int num) |
| int | set_number_of_force_sensors (int num) |
| int | set_number_of_gyro_sensors (int num) |
| int | set_number_of_joints (int num) |
| int | set_signal_period (long period_ns) |
| static void | setJointStates (const RobotState::ConstPtr &_js) |
| void | timespec_add_ns (timespec *ts, long ns) |
| double | timespec_compare (timespec *ts1, timespec *ts2) |
| int | unlock_iob () |
| int | wait_for_iob_signal () |
| int | write_accelerometer_offset (int id, double *offset) |
| int | write_angle_offset (int id, double angle) |
| int | write_attitude_sensor_offset (int id, double *offset) |
| int | write_command_angle (int id, double angle) |
| int | write_command_angles (const double *angles) |
| int | write_command_torque (int id, double torque) |
| int | write_command_velocities (double *vels) |
| int | write_command_velocity (int id, double vel) |
| int | write_control_mode (int id, joint_control_mode s) |
| int | write_dgain (int id, double gain) |
| int | write_digital_output (const char *doutput) |
| int | write_dio (unsigned short buf) |
| int | write_force_offset (int id, double *offsets) |
| int | write_gyro_sensor_offset (int id, double *offset) |
| int | write_pgain (int id, double gain) |
| int | write_power_command (int id, int com) |
| int | write_servo (int id, int com) |
Variables | |
| static std::vector < std::vector< double > > | accel_offset |
| static std::vector < std::vector< double > > | accelerometers |
| static std::vector < std::vector< double > > | attitude_sensors |
| static std::vector< double > | command |
| static std::vector < std::vector< double > > | force_offset |
| static std::vector < std::vector< double > > | forces |
| static int | frame = 0 |
| static long | g_period_ns = 1000000 |
| static timespec | g_ts |
| static std::vector < std::vector< double > > | gyro_offset |
| static std::vector < std::vector< double > > | gyros |
| static int | init_sub_flag = FALSE |
| static JointCommand | initial_jointcommand |
| static bool | isLocked = false |
| static std::map< int, int > | joint_model2real_map |
| static std::vector< int > | joint_real2model_vec |
| static JointCommand | jointcommand |
| static RobotState | js |
| static std::vector< int > | power |
| static std::vector< double > | prev_command |
| static ros::Publisher | pub_joint_command |
| static ros::Time | rg_ts |
| static ros::NodeHandle * | rosnode |
| static std::vector< int > | servo |
| static ros::Subscriber | sub_robot_state |
| #define CHECK_ACCELEROMETER_ID | ( | id | ) | if ((id) < 0 || (id) >= number_of_accelerometers()) return E_ID |
| #define CHECK_ATTITUDE_SENSOR_ID | ( | id | ) | if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID |
| #define CHECK_FORCE_SENSOR_ID | ( | id | ) | if ((id) < 0 || (id) >= number_of_force_sensors()) return E_ID |
| #define CHECK_GYRO_SENSOR_ID | ( | id | ) | if ((id) < 0 || (id) >= number_of_gyro_sensors()) return E_ID |
| #define CHECK_JOINT_ID | ( | id | ) | if ((id) < 0 || (id) >= number_of_joints()) return E_ID |
| #define JOINT_ID_MODEL2REAL | ( | id | ) | joint_id_model2real(id) |
| #define JOINT_ID_REAL2MODEL | ( | id | ) | joint_real2model_vec[id] |
| #define NUM_OF_REAL_JOINT (joint_real2model_vec.size()) |
| long get_signal_period | ( | ) |
| int initializeJointAngle | ( | const char * | name, |
| const char * | option | ||
| ) |
| int joint_calibration | ( | int | id, |
| double | angle | ||
| ) |
| static int joint_id_model2real | ( | int | id | ) | [inline, static] |
| int length_digital_input | ( | void | ) |
| int length_digital_output | ( | void | ) |
| size_t length_of_extra_servo_state | ( | int | id | ) |
| int number_of_accelerometers | ( | ) |
| int number_of_attitude_sensors | ( | ) |
| int number_of_force_sensors | ( | ) |
| int number_of_gyro_sensors | ( | ) |
| int number_of_joints | ( | ) |
| int number_of_substeps | ( | ) |
| int read_accelerometer | ( | int | id, |
| double * | accels | ||
| ) |
| int read_accelerometer_offset | ( | int | id, |
| double * | offset | ||
| ) |
| int read_actual_angle | ( | int | id, |
| double * | angle | ||
| ) |
| int read_actual_angles | ( | double * | angles | ) |
| int read_actual_torques | ( | double * | torques | ) |
| int read_angle_offset | ( | int | id, |
| double * | angle | ||
| ) |
| int read_attitude_sensor | ( | int | id, |
| double * | att | ||
| ) |
| int read_calib_state | ( | int | id, |
| int * | s | ||
| ) |
| int read_command_angle | ( | int | id, |
| double * | angle | ||
| ) |
| int read_command_angles | ( | double * | angles | ) |
| int read_command_torque | ( | int | id, |
| double * | torque | ||
| ) |
| int read_command_torques | ( | double * | torques | ) |
| int read_command_velocities | ( | double * | vels | ) |
| int read_command_velocity | ( | int | id, |
| double * | vel | ||
| ) |
| int read_control_mode | ( | int | id, |
| joint_control_mode * | s | ||
| ) |
| int read_current | ( | int | id, |
| double * | mcurrent | ||
| ) |
| int read_current_limit | ( | int | id, |
| double * | v | ||
| ) |
| int read_currents | ( | double * | currents | ) |
| int read_dgain | ( | int | id, |
| double * | gain | ||
| ) |
| int read_digital_input | ( | char * | dinput | ) |
| int read_driver_temperature | ( | int | id, |
| unsigned char * | v | ||
| ) |
| int read_encoder_pulse | ( | int | id, |
| double * | ec | ||
| ) |
| int read_extra_servo_state | ( | int | id, |
| int * | state | ||
| ) |
| int read_force_offset | ( | int | id, |
| double * | offsets | ||
| ) |
| int read_force_sensor | ( | int | id, |
| double * | forces | ||
| ) |
| int read_gauges | ( | double * | gauges | ) |
| int read_gear_ratio | ( | int | id, |
| double * | gr | ||
| ) |
| int read_gyro_sensor | ( | int | id, |
| double * | rates | ||
| ) |
| int read_gyro_sensor_offset | ( | int | id, |
| double * | offset | ||
| ) |
| unsigned long long read_iob_frame | ( | ) |
| int read_limit_angle | ( | int | id, |
| double * | angle | ||
| ) |
| int read_llimit_angle | ( | int | id, |
| double * | angle | ||
| ) |
| int read_lock_owner | ( | pid_t * | pid | ) |
| int read_pgain | ( | int | id, |
| double * | gain | ||
| ) |
| int read_power | ( | double * | voltage, |
| double * | current | ||
| ) |
| int read_power_command | ( | int | id, |
| int * | com | ||
| ) |
| int read_power_state | ( | int | id, |
| int * | s | ||
| ) |
| int read_servo_alarm | ( | int | id, |
| int * | a | ||
| ) |
| int read_servo_state | ( | int | id, |
| int * | s | ||
| ) |
| int read_temperature | ( | int | id, |
| double * | v | ||
| ) |
| int read_torque_const | ( | int | id, |
| double * | tc | ||
| ) |
| int read_torque_limit | ( | int | id, |
| double * | limit | ||
| ) |
| int read_touch_sensors | ( | unsigned short * | onoff | ) |
| int read_ulimit_angle | ( | int | id, |
| double * | angle | ||
| ) |
| int reset_body | ( | void | ) |
| int set_number_of_accelerometers | ( | int | num | ) |
| int set_number_of_attitude_sensors | ( | int | num | ) |
| int set_number_of_force_sensors | ( | int | num | ) |
| int set_number_of_gyro_sensors | ( | int | num | ) |
| int set_number_of_joints | ( | int | num | ) |
| int set_signal_period | ( | long | period_ns | ) |
| static void setJointStates | ( | const RobotState::ConstPtr & | _js | ) | [static] |
| void timespec_add_ns | ( | timespec * | ts, |
| long | ns | ||
| ) |
| double timespec_compare | ( | timespec * | ts1, |
| timespec * | ts2 | ||
| ) |
| int unlock_iob | ( | ) |
| int wait_for_iob_signal | ( | ) |
| int write_accelerometer_offset | ( | int | id, |
| double * | offset | ||
| ) |
| int write_angle_offset | ( | int | id, |
| double | angle | ||
| ) |
| int write_attitude_sensor_offset | ( | int | id, |
| double * | offset | ||
| ) |
| int write_command_angle | ( | int | id, |
| double | angle | ||
| ) |
| int write_command_angles | ( | const double * | angles | ) |
| int write_command_torque | ( | int | id, |
| double | torque | ||
| ) |
| int write_command_velocities | ( | double * | vels | ) |
| int write_command_velocity | ( | int | id, |
| double | vel | ||
| ) |
| int write_control_mode | ( | int | id, |
| joint_control_mode | s | ||
| ) |
| int write_dgain | ( | int | id, |
| double | gain | ||
| ) |
| int write_digital_output | ( | const char * | doutput | ) |
| int write_force_offset | ( | int | id, |
| double * | offsets | ||
| ) |
| int write_gyro_sensor_offset | ( | int | id, |
| double * | offset | ||
| ) |
| int write_pgain | ( | int | id, |
| double | gain | ||
| ) |
| int write_power_command | ( | int | id, |
| int | com | ||
| ) |
| int write_servo | ( | int | id, |
| int | com | ||
| ) |
std::vector<std::vector<double> > accel_offset [static] |
std::vector<std::vector<double> > accelerometers [static] |
std::vector<std::vector<double> > attitude_sensors [static] |
std::vector<std::vector<double> > force_offset [static] |
long g_period_ns = 1000000 [static] |
std::vector<std::vector<double> > gyro_offset [static] |
int init_sub_flag = FALSE [static] |
JointCommand initial_jointcommand [static] |
std::map<int, int> joint_model2real_map [static] |
std::vector<int> joint_real2model_vec [static] |
JointCommand jointcommand [static] |
RobotState js [static] |
std::vector<double> prev_command [static] |
ros::Publisher pub_joint_command [static] |
ros::NodeHandle* rosnode [static] |
ros::Subscriber sub_robot_state [static] |