Defines | Functions | Variables
iob.cpp File Reference
#include <unistd.h>
#include <iostream>
#include <cstdlib>
#include <cstring>
#include <vector>
#include "iob.h"
Include dependency graph for iob.cpp:

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

Functions

int close_iob (void)
 close connection with joint servo process
long get_signal_period ()
 get the period of signals issued by wait_for_iob_signal()
int initializeJointAngle (const char *name, const char *option)
 initialize joint angle
int joint_calibration (int id, double angle)
int length_digital_input ()
 get_digital_input_length
int length_digital_output ()
 get_digital_output_length
size_t length_of_extra_servo_state (int id)
 get length of extra servo states
int lock_iob ()
 lock access to iob
int number_of_accelerometers ()
 get the number of accelerometers
int number_of_attitude_sensors ()
 get the number of attitude sensors
int number_of_force_sensors ()
 get the number of force sensors
int number_of_gyro_sensors ()
 get the number of gyro sensors
int number_of_joints ()
 get the number of joints
int number_of_substeps ()
int open_iob (void)
 open connection with joint servo process
int read_accelerometer (int id, double *accels)
 read output of accelerometer
int read_accelerometer_offset (int id, double *offset)
 read offset values for accelerometer output
int read_actual_angle (int id, double *angle)
 read current joint angle[rad]
int read_actual_angles (double *angles)
 read array of current joint angles[rad]
int read_actual_torques (double *torques)
 read array of current joint torques[Nm]
int read_actual_velocities (double *vels)
 read actual angular velocities[rad/s]
int read_actual_velocity (int id, double *vel)
 read actual angular velocity[rad/s]
int read_angle_offset (int id, double *angle)
 read offset value for joint[rad]
int read_attitude_sensor (int id, double *att)
 read output of attitude sensor
int read_calib_state (int id, int *s)
 read callibration state of joint
int read_command_angle (int id, double *angle)
 read command angle[rad]
int read_command_angles (double *angles)
 read array of command angles[rad]
int read_command_torque (int id, double *torque)
 read command torque[Nm]
int read_command_torques (double *torques)
 read array of command torques[Nm]
int read_command_velocities (double *vels)
 read command angular velocities[rad/s]
int read_command_velocity (int id, double *vel)
 read command angular velocity[rad/s]
int read_control_mode (int id, joint_control_mode *s)
 read joint control mode
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)
 read D gain[Nm/(rad/s)]
int read_digital_input (char *dinput)
 read_digital_input, non-applicable bits are nop
int read_digital_output (char *doutput)
 read_digital_output, non-applicable bits are nop
int read_driver_temperature (int id, unsigned char *v)
 read temperature of motor driver[Celsius]
int read_encoder_pulse (int id, double *ec)
int read_extra_servo_state (int id, int *state)
 read extra servo states
int read_force_offset (int id, double *offsets)
 read offset values for force sensor output
int read_force_sensor (int id, double *forces)
 read output of force sensor
int read_gauges (double *gauges)
int read_gear_ratio (int id, double *gr)
int read_gyro_sensor (int id, double *rates)
 read output of gyro sensor
int read_gyro_sensor_offset (int id, double *offset)
 read offset values for gyro sensor output
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)
 read id of the process whic is locking access to iob
int read_pgain (int id, double *gain)
 read P gain[Nm/rad]
int read_power (double *voltage, double *current)
 read status of power source
int read_power_command (int id, int *com)
 turn on/off power supply for motor driver
int read_power_state (int id, int *s)
 read power status of motor driver
int read_servo_alarm (int id, int *a)
 read servo alarms
int read_servo_state (int id, int *s)
 read servo status
int read_temperature (int id, double *v)
 read thermometer
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)
 set the number of accelerometers
int set_number_of_attitude_sensors (int num)
int set_number_of_force_sensors (int num)
 set the number of force sensors
int set_number_of_gyro_sensors (int num)
 set the number of gyro sensors
int set_number_of_joints (int num)
 set the number of joints
int set_signal_period (long period_ns)
 set the period of signals issued by wait_for_iob_signal()
void timespec_add_ns (timespec *ts, long ns)
double timespec_compare (timespec *ts1, timespec *ts2)
int unlock_iob ()
 unlock access to iob
int wait_for_iob_signal ()
 wait until iob signal is issued
int write_accelerometer_offset (int id, double *offset)
 write offset values for accelerometer output
int write_angle_offset (int id, double angle)
 write offset value for joint[rad]
int write_attitude_sensor_offset (int id, double *offset)
int write_command_angle (int id, double angle)
 write command angle[rad]
int write_command_angles (const double *angles)
 write array of command angles[rad]
int write_command_torque (int id, double torque)
 write command torque[Nm]
int write_command_torques (const double *torques)
 write array of command torques[Nm]
int write_command_velocities (const double *vels)
 write command angular velocities[rad/s]
int write_command_velocity (int id, double vel)
 write command angular velocity[rad/s]
int write_control_mode (int id, joint_control_mode s)
 write joint control mode
int write_dgain (int id, double gain)
 write D gain[Nm/(rad/s)]
int write_digital_output (const char *doutput)
 write_digital_output, non-applicable bits are nop
int write_digital_output_with_mask (const char *doutput, const char *mask)
 write_digital_output, non-applicable bits are nop
int write_dio (unsigned short buf)
int write_force_offset (int id, double *offsets)
 write offset values for force sensor output
int write_gyro_sensor_offset (int id, double *offset)
 write offset values for gyro sensor output
int write_pgain (int id, double gain)
 write P gain[Nm/rad]
int write_power_command (int id, int com)
 turn on/off power supply for motor driver
int write_servo (int id, int com)
 turn on/off joint servo

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 = 5000000
static timespec g_ts
static std::vector
< std::vector< double > > 
gyro_offset
static std::vector
< std::vector< double > > 
gyros
static bool isLocked = false
static std::vector< intpower
static std::vector< intservo

Define Documentation

#define CHECK_ACCELEROMETER_ID (   id)    if ((id) < 0 || (id) >= number_of_accelerometers()) return E_ID

Definition at line 25 of file iob.cpp.

#define CHECK_ATTITUDE_SENSOR_ID (   id)    if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID

Definition at line 27 of file iob.cpp.

#define CHECK_FORCE_SENSOR_ID (   id)    if ((id) < 0 || (id) >= number_of_force_sensors()) return E_ID

Definition at line 24 of file iob.cpp.

#define CHECK_GYRO_SENSOR_ID (   id)    if ((id) < 0 || (id) >= number_of_gyro_sensors()) return E_ID

Definition at line 26 of file iob.cpp.

#define CHECK_JOINT_ID (   id)    if ((id) < 0 || (id) >= number_of_joints()) return E_ID

Definition at line 23 of file iob.cpp.


Function Documentation

close connection with joint servo process

Return values:
TRUEclosed successfully
FALSEotherwise

Definition at line 415 of file iob.cpp.

get the period of signals issued by wait_for_iob_signal()

Returns:
the period of signals[ns]

Definition at line 647 of file iob.cpp.

int initializeJointAngle ( const char *  name,
const char *  option 
)

initialize joint angle

Parameters:
namejoint name, part name or "all"
optionstring of joint angle initialization
Returns:
TRUE if initialized successfully, FALSE otherwise

Definition at line 652 of file iob.cpp.

int joint_calibration ( int  id,
double  angle 
)

Definition at line 429 of file iob.cpp.

get_digital_input_length

Returns:
length of digital input in bytes

Definition at line 663 of file iob.cpp.

get_digital_output_length

Returns:
length of digital output in bytes

Definition at line 678 of file iob.cpp.

get length of extra servo states

Parameters:
idjoint id
Returns:
length of extra servo states

Definition at line 631 of file iob.cpp.

lock access to iob

Return values:
TRUEiob is locked successfully
FALSEsome other process is locking iob

Definition at line 495 of file iob.cpp.

get the number of accelerometers

Returns:
the number of accelerometers

Definition at line 82 of file iob.cpp.

get the number of attitude sensors

Returns:
the number of attitude sensors

Definition at line 87 of file iob.cpp.

get the number of force sensors

Returns:
the number of force sensors

Definition at line 72 of file iob.cpp.

get the number of gyro sensors

Returns:
the number of gyro sensors

Definition at line 77 of file iob.cpp.

get the number of joints

Returns:
the number of joints

Definition at line 67 of file iob.cpp.

Returns:
the number of substeps

Definition at line 560 of file iob.cpp.

int open_iob ( void  )

open connection with joint servo process

Return values:
TRUEopened successfully
FALSEotherwise

Definition at line 403 of file iob.cpp.

int read_accelerometer ( int  id,
double *  accels 
)

read output of accelerometer

Parameters:
idaccelerometer id
accelsaccelerations [m/s^2], length of array must be 3
Returns:
TRUE or E_ID

Definition at line 317 of file iob.cpp.

int read_accelerometer_offset ( int  id,
double *  offset 
)

read offset values for accelerometer output

Parameters:
idaccelerometer id
offsetoffset values[rad/s^2], length of array must be 3.
Return values:
TRUEoffset values are read successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 450 of file iob.cpp.

int read_actual_angle ( int  id,
double *  angle 
)

read current joint angle[rad]

Parameters:
idjoint id
angleactual joint angle[rad]
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 208 of file iob.cpp.

int read_actual_angles ( double *  angles)

read array of current joint angles[rad]

Parameters:
anglesarray of joint angle[rad], length of array must be equal to number_of_joints()
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 215 of file iob.cpp.

int read_actual_torques ( double *  torques)

read array of current joint torques[Nm]

Parameters:
torquesarray of actual joint torque[Nm], length of array must be equal to number_of_joints()
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 223 of file iob.cpp.

int read_actual_velocities ( double *  vels)

read actual angular velocities[rad/s]

Parameters:
velsarray of angular velocity [rad/s]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 372 of file iob.cpp.

int read_actual_velocity ( int  id,
double *  vel 
)

read actual angular velocity[rad/s]

Parameters:
idjoint id
velangular velocity [rad/s]
Returns:
TRUE or E_ID

Definition at line 357 of file iob.cpp.

int read_angle_offset ( int  id,
double *  offset 
)

read offset value for joint[rad]

Parameters:
idjoint id
offsetoffset value[rad]
Return values:
TRUEoffset value is read successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 518 of file iob.cpp.

int read_attitude_sensor ( int  id,
double *  att 
)

read output of attitude sensor

Parameters:
idattitude sensor id
attroll-pitch-yaw angle[rad], length of array must be 3
Return values:
TRUEsensor values are read successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 332 of file iob.cpp.

int read_calib_state ( int  id,
int s 
)

read callibration state of joint

Parameters:
idjoint id
sTRUE if calibration is already done, FALSE otherwise
Return values:
TRUEcalibration status is read successfully
E_IDinvalid joint id is specified
FALSEthis function is not supported

Definition at line 487 of file iob.cpp.

int read_command_angle ( int  id,
double *  angle 
)

read command angle[rad]

Parameters:
idjoint id
anglecommand joint angle[rad]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 248 of file iob.cpp.

int read_command_angles ( double *  angles)

read array of command angles[rad]

Parameters:
anglesarray of joint angles[rad], length of array must equal to DOF
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 262 of file iob.cpp.

int read_command_torque ( int  id,
double *  torque 
)

read command torque[Nm]

Parameters:
idjoint id
torquejoint torque[Nm]
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 228 of file iob.cpp.

int read_command_torques ( double *  torques)

read array of command torques[Nm]

Parameters:
torquesarray of command torques[Nm]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 238 of file iob.cpp.

int read_command_velocities ( double *  vels)

read command angular velocities[rad/s]

Parameters:
velsarray of angular velocity [rad/s]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 377 of file iob.cpp.

int read_command_velocity ( int  id,
double *  vel 
)

read command angular velocity[rad/s]

Parameters:
idjoint id
velangular velocity [rad/s]
Returns:
TRUE or E_ID

Definition at line 362 of file iob.cpp.

read joint control mode

Parameters:
idjoint id
sjoint control mode
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 195 of file iob.cpp.

int read_current ( int  id,
double *  mcurrent 
)

Definition at line 337 of file iob.cpp.

int read_current_limit ( int  id,
double *  v 
)

Definition at line 342 of file iob.cpp.

int read_currents ( double *  currents)

Definition at line 347 of file iob.cpp.

int read_dgain ( int  id,
double *  gain 
)

read D gain[Nm/(rad/s)]

Parameters:
idjoint id
gainD gain[Nm/(rad/s)]
Returns:
TRUE or E_ID

Definition at line 288 of file iob.cpp.

int read_digital_input ( char *  dinput)

read_digital_input, non-applicable bits are nop

Parameters:
dinputdigital input from environment
Returns:
TRUE if applicable, FALSE otherwise

Definition at line 658 of file iob.cpp.

int read_digital_output ( char *  doutput)

read_digital_output, non-applicable bits are nop

Parameters:
doutputdigital output to environment
Returns:
TRUE if applicable, FALSE otherwise

Definition at line 683 of file iob.cpp.

int read_driver_temperature ( int  id,
unsigned char *  v 
)

read temperature of motor driver[Celsius]

Parameters:
idjoint id
vtemperature[Celsius]
Return values:
TRUEtemperature is read successfully
E_IDinvalid joint id is specified
FALSEthis function is not supported

Definition at line 593 of file iob.cpp.

int read_encoder_pulse ( int  id,
double *  ec 
)

Definition at line 536 of file iob.cpp.

int read_extra_servo_state ( int  id,
int state 
)

read extra servo states

Parameters:
idjoint id
statearray of int where extra servo states are stored
Returns:
TRUE if read successfully, FALSE otherwise

Definition at line 636 of file iob.cpp.

int read_force_offset ( int  id,
double *  offsets 
)

read offset values for force sensor output

Parameters:
idforce/torque sensor id
offsetsoffset values[N][Nm], length of array must be 6.
Return values:
TRUEoffset values are read successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 466 of file iob.cpp.

int read_force_sensor ( int  id,
double *  forces 
)

read output of force sensor

Parameters:
idForce Sensor id
forcesarray of forces[N] and moments[Nm], length of array must be 6
Returns:
TRUE or E_ID

Definition at line 298 of file iob.cpp.

int read_gauges ( double *  gauges)

Definition at line 352 of file iob.cpp.

int read_gear_ratio ( int  id,
double *  gr 
)

Definition at line 540 of file iob.cpp.

int read_gyro_sensor ( int  id,
double *  rates 
)

read output of gyro sensor

Parameters:
idgyro sensor id
ratesangular velocities [rad/s], length of array must be 3
Returns:
TRUE or E_ID

Definition at line 307 of file iob.cpp.

int read_gyro_sensor_offset ( int  id,
double *  offset 
)

read offset values for gyro sensor output

Parameters:
idgyro sensor id
offsetoffset values[rad/s], length of array must be 3.
Return values:
TRUEoffset values are read successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 434 of file iob.cpp.

unsigned long long read_iob_frame ( )

Definition at line 553 of file iob.cpp.

int read_limit_angle ( int  id,
double *  angle 
)

Definition at line 513 of file iob.cpp.

int read_llimit_angle ( int  id,
double *  angle 
)

Definition at line 532 of file iob.cpp.

read id of the process whic is locking access to iob

Definition at line 508 of file iob.cpp.

int read_pgain ( int  id,
double *  gain 
)

read P gain[Nm/rad]

Parameters:
idjoint id
gainP gain[Nm/rad]
Returns:
TRUE or E_ID

Definition at line 278 of file iob.cpp.

int read_power ( double *  v,
double *  a 
)

read status of power source

Parameters:
vvoltage[V]
acurrent[A]
Returns:
TRUE or FALSE

Definition at line 565 of file iob.cpp.

int read_power_command ( int  id,
int com 
)

turn on/off power supply for motor driver

Parameters:
idjoint id
comON/OFF
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 174 of file iob.cpp.

int read_power_state ( int  id,
int s 
)

read power status of motor driver

Parameters:
idjoint id
sON or OFF is returned
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 160 of file iob.cpp.

int read_servo_alarm ( int  id,
int a 
)

read servo alarms

Parameters:
idjoint id
aservo alarms
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 188 of file iob.cpp.

int read_servo_state ( int  id,
int s 
)

read servo status

Parameters:
idjoint id
sON/OFF
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 181 of file iob.cpp.

int read_temperature ( int  id,
double *  v 
)

read thermometer

Parameters:
idid of thermometer
vtemperature[Celsius]
Return values:
TRUEtemperature is read successfully
E_IDinvalid thermometer id is specified
FALSEthis function is not supported

Definition at line 387 of file iob.cpp.

int read_torque_const ( int  id,
double *  tc 
)

Definition at line 544 of file iob.cpp.

int read_torque_limit ( int  id,
double *  limit 
)

Definition at line 548 of file iob.cpp.

int read_touch_sensors ( unsigned short *  onoff)

Definition at line 327 of file iob.cpp.

int read_ulimit_angle ( int  id,
double *  angle 
)

Definition at line 528 of file iob.cpp.

Definition at line 421 of file iob.cpp.

set the number of accelerometers

Parameters:
numthe number of accelerometers
Returns:
TRUE if the number of accelerometers is set, FALSE otherwise

Definition at line 133 of file iob.cpp.

Definition at line 148 of file iob.cpp.

set the number of force sensors

Parameters:
numthe number of force sensors
Returns:
TRUE if the number of force sensors is set, FALSE otherwise

Definition at line 103 of file iob.cpp.

set the number of gyro sensors

Parameters:
numthe number of gyro sensors
Returns:
TRUE if the number of gyro sensors is set, FALSE otherwise

Definition at line 118 of file iob.cpp.

set the number of joints

Parameters:
numthe number of joints
Returns:
TRUE if the number of joints is set, FALSE otherwise

Definition at line 92 of file iob.cpp.

int set_signal_period ( long  period_ns)

set the period of signals issued by wait_for_iob_signal()

Parameters:
period_nsthe period of signals[ns]
Returns:
TRUE if set successfully, FALSE otherwise

Definition at line 641 of file iob.cpp.

void timespec_add_ns ( timespec *  ts,
long  ns 
)

Definition at line 599 of file iob.cpp.

double timespec_compare ( timespec *  ts1,
timespec *  ts2 
)

Definition at line 608 of file iob.cpp.

unlock access to iob

Definition at line 502 of file iob.cpp.

wait until iob signal is issued

Returns:
TRUE if signal is received successfully, FALSE otherwise

Definition at line 615 of file iob.cpp.

int write_accelerometer_offset ( int  id,
double *  offset 
)

write offset values for accelerometer output

Parameters:
idaccelerometer id
offsetoffset values[rad/s^2], length of array must be 3.
Return values:
TRUEoffset values are written successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 458 of file iob.cpp.

int write_angle_offset ( int  id,
double  offset 
)

write offset value for joint[rad]

Parameters:
idjoint id
offsetoffset value[rad]
Return values:
TRUEoffset values are written successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 523 of file iob.cpp.

int write_attitude_sensor_offset ( int  id,
double *  offset 
)

Definition at line 482 of file iob.cpp.

int write_command_angle ( int  id,
double  angle 
)

write command angle[rad]

Parameters:
idjoint id
anglecommand joint angle[rad]
Returns:
TRUE or E_ID

Definition at line 255 of file iob.cpp.

int write_command_angles ( const double *  angles)

write array of command angles[rad]

Parameters:
anglesarray of joint angles[rad], length of array must equal to DOF
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 270 of file iob.cpp.

int write_command_torque ( int  id,
double  torque 
)

write command torque[Nm]

Parameters:
idjoint id
torquejoint torque[Nm]
Returns:
TRUE if this function is supported, FALSE otherwise

Definition at line 233 of file iob.cpp.

int write_command_torques ( const double *  torques)

write array of command torques[Nm]

Parameters:
torquesarray of command torques[Nm]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 243 of file iob.cpp.

int write_command_velocities ( const double *  vels)

write command angular velocities[rad/s]

Parameters:
velsarray of angular velocity [rad/s]
Return values:
TRUEthis function is supported
FALSEotherwise

Definition at line 382 of file iob.cpp.

int write_command_velocity ( int  id,
double  vel 
)

write command angular velocity[rad/s]

Parameters:
idjoint id
velangular velocity [rad/s]
Returns:
TRUE or E_ID

Definition at line 367 of file iob.cpp.

write joint control mode

Parameters:
idjoint id
sjoint control mode
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 202 of file iob.cpp.

int write_dgain ( int  id,
double  gain 
)

write D gain[Nm/(rad/s)]

Parameters:
idjoint id
gainD gain[Nm/(rad/s)]
Returns:
TRUE or E_ID

Definition at line 293 of file iob.cpp.

int write_digital_output ( const char *  doutput)

write_digital_output, non-applicable bits are nop

Parameters:
doutputset digital output to environment
Returns:
TRUE if applicable, FALSE otherwise

Definition at line 668 of file iob.cpp.

int write_digital_output_with_mask ( const char *  doutput,
const char *  dmask 
)

write_digital_output, non-applicable bits are nop

Parameters:
doutputset digital output to environment
maskbinary vector which selects output to be set
Returns:
TRUE if applicable, FALSE otherwise

Definition at line 673 of file iob.cpp.

int write_dio ( unsigned short  buf)

Definition at line 398 of file iob.cpp.

int write_force_offset ( int  id,
double *  offsets 
)

write offset values for force sensor output

Parameters:
idforce/torque id
offsetsoffset values[N][Nm], length of array must be 6.
Return values:
TRUEoffset values are written successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 474 of file iob.cpp.

int write_gyro_sensor_offset ( int  id,
double *  offset 
)

write offset values for gyro sensor output

Parameters:
idgyro sensor id
offsetoffset values[rad/s], length of array must be 3.
Return values:
TRUEoffset values are written successfully
E_IDinvalid id is specified
FALSEthis function is not supported

Definition at line 442 of file iob.cpp.

int write_pgain ( int  id,
double  gain 
)

write P gain[Nm/rad]

Parameters:
idjoint id
gainP gain[Nm/rad]
Returns:
TRUE or E_ID

Definition at line 283 of file iob.cpp.

int write_power_command ( int  id,
int  com 
)

turn on/off power supply for motor driver

Parameters:
idjoint id
comON/OFF
Return values:
TRUEthis function is supported
E_IDinvalid joint id is specified
FALSEotherwise

Definition at line 167 of file iob.cpp.

int write_servo ( int  id,
int  com 
)

turn on/off joint servo

Parameters:
idjoint id
comON/OFF
Returns:
TRUE if this function is supported, FALSE otherwise

Definition at line 392 of file iob.cpp.


Variable Documentation

std::vector<std::vector<double> > accel_offset [static]

Definition at line 15 of file iob.cpp.

std::vector<std::vector<double> > accelerometers [static]

Definition at line 11 of file iob.cpp.

std::vector<std::vector<double> > attitude_sensors [static]

Definition at line 12 of file iob.cpp.

std::vector<double> command [static]

Definition at line 8 of file iob.cpp.

std::vector<std::vector<double> > force_offset [static]

Definition at line 13 of file iob.cpp.

std::vector<std::vector<double> > forces [static]

Definition at line 9 of file iob.cpp.

int frame = 0 [static]

Definition at line 19 of file iob.cpp.

long g_period_ns = 5000000 [static]

Definition at line 21 of file iob.cpp.

timespec g_ts [static]

Definition at line 20 of file iob.cpp.

std::vector<std::vector<double> > gyro_offset [static]

Definition at line 14 of file iob.cpp.

std::vector<std::vector<double> > gyros [static]

Definition at line 10 of file iob.cpp.

bool isLocked = false [static]

Definition at line 18 of file iob.cpp.

std::vector<int> power [static]

Definition at line 16 of file iob.cpp.

std::vector<int> servo [static]

Definition at line 17 of file iob.cpp.



hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:57