C++ API and example

1. Introduction

This page exist in order to extract the examples from the Doxygen documentation, Please have look at the end of this page there are all the examples.

2. C++ API and example

class AnalogSensor : public blmc_drivers::AnalogSensorInterface, public blmc_drivers::AnalogSensorInterface
#include <analog_sensor.hpp>

AnalogSensor class is the implementation of the above interface.

Public Functions

AnalogSensor(std::shared_ptr<MotorBoardInterface> board, bool sensor_id)

Construct a new AnalogSensor object.

Parameters:
  • board – is a motor board which gives access to the motor sensors (position, velocity, current, etc) and to the motor cotrols.

  • sensor_id – is the id of the sensor on the control board

virtual std::shared_ptr<const ScalarTimeseries> get_measurement() const

Get the measurement object which is the list of time stamped data.

Returns:

std::shared_ptr<const ScalarTimeseries> which is a pointer to the a list of time stamped data

AnalogSensor(std::shared_ptr<MotorBoardInterface> board, bool sensor_id)

Construct a new AnalogSensor object.

Parameters:
  • board – is a motor board which gives access to the motor sensors (position, velocity, current, etc) and to the motor cotrols.

  • sensor_id – is the id of the sensor on the control board

virtual std::shared_ptr<const ScalarTimeseries> get_measurement() const

Get the measurement object which is the list of time stamped data.

Returns:

std::shared_ptr<const ScalarTimeseries> which is a pointer to the a list of time stamped data

Private Members

std::shared_ptr<MotorBoardInterface> board_

board_ is the measurement object, it caontains the list of the timed stamped data

size_t sensor_id_

sensor_id_ is the identification number of the sensor on the control board, for now it is either 0 or 1

class AnalogSensorInterface : public blmc_drivers::DeviceInterface, public blmc_drivers::DeviceInterface
#include <analog_sensor.hpp>

AnalogSensorInterface class is a simple abstract interface for using blmc analog measurements.

Subclassed by blmc_drivers::AnalogSensor, blmc_drivers::AnalogSensor

Public Types

typedef time_series::TimeSeries<double> ScalarTimeseries

This is just a short cut for the time series types.

typedef time_series::TimeSeries<double> ScalarTimeseries

This is just a short cut for the time series types.

Public Functions

virtual std::shared_ptr<const ScalarTimeseries> get_measurement() const = 0

Get the measurement object which is the list of time stamped data.

Returns:

std::shared_ptr<const ScalarTimeseries> which is a pointer to the a list of time stamped data

inline virtual ~AnalogSensorInterface()

Destroy the AnalogSensorInterface object.

virtual std::shared_ptr<const ScalarTimeseries> get_measurement() const = 0

Get the measurement object which is the list of time stamped data.

Returns:

std::shared_ptr<const ScalarTimeseries> which is a pointer to the a list of time stamped data

inline virtual ~AnalogSensorInterface()

Destroy the AnalogSensorInterface object.

class BlmcJointModule

The BlmcJointModule class is containing the joint information.

It is here to help converting the data from the motor side to the joint side. It also allows the calibration of the joint position during initialization.

Public Functions

BlmcJointModule(std::shared_ptr<blmc_drivers::MotorInterface> motor, const double &motor_constant, const double &gear_ratio, const double &zero_angle, const bool &reverse_polarity = false, const double &max_current = 2.1)

Construct a new BlmcJointModule object.

Parameters:
  • motor – is the C++ object allowing us to send commands and receive sensor data.

  • motor_constant – ( \( k \)) is the torque constant of the motor \( \tau_{motor} = k * i_{motor} \)

  • gear_ratio – is the gear ratio between the motor and the joint.

  • zero_angle – is the angle between the closest positive motor index and the zero configuration.

  • reverse_polarity

  • max_current

void set_torque(const double &desired_torque)

Set the joint torque to be sent.

Parameters:

desired_torque – (Nm)

void set_zero_angle(const double &zero_angle)

Set the zero_angle.

The zero_angle is the angle between the closest positive motor index and the zero configuration.

Parameters:

zero_angle – (rad)

void set_joint_polarity(const bool &reverse_polarity)

Define if the motor should turn clock-wize or counter clock-wize.

Parameters:

reverse_polarity – true:reverse rotation axis, false:do nothing.

void send_torque()

send the joint torque to the motor.

The conversion between joint torque and motor current is done automatically.

double get_max_torque() const

Get the maximum admissible joint torque that can be applied.

Returns:

double

double get_sent_torque() const

Get the sent joint torque.

Returns:

double (Nm).

double get_measured_torque() const

Get the measured joint torque.

Returns:

double (Nm).

double get_measured_angle() const

Get the measured angle of the joint.

Returns:

double (rad).

double get_measured_velocity() const

Get the measured velocity of the joint.

This data is computed on board of the control card.

Returns:

double (rad/s).

double get_measured_index_angle() const

Get the measured index angle.

There is one index per motor rotation so there are gear_ratio indexes per joint rotation.

Returns:

double (rad).

double get_zero_angle() const

Get the zero_angle_.

These are the angle between the starting pose and the theoretical zero pose.

Returns:

double (rad).

void set_position_control_gains(double kp, double kd)

Set control gains for PD position controller.

Parameters:
  • kp – P gain ( (Nm) / rad ).

  • kd – D gain ( (Nm) / (rad/s) ).

double execute_position_controller(double target_position_rad) const

Execute one iteration of the position controller.

Parameters:

target_position_rad – Target position (rad).

Returns:

Torque command (Nm).

bool calibrate(double &angle_zero_to_index, double &index_angle, bool mechanical_calibration = false)

This method calibrate the joint position knowing the angle between the closest (in positive torque) motor index and the theoretical zero pose.

Deprecated:

!!!!!!!

Warning, this method should be called in a real time thread!

Param :

void homing_at_current_position(double home_offset_rad)

Set zero position relative to current position.

Parameters:

home_offset_rad – Offset from home position to zero position. Unit: radian.

void init_homing(int joint_id, double search_distance_limit_rad, double home_offset_rad, double profile_step_size_rad = 0.001)

Initialize the homing procedure.

This has to be called before update_homing().

Parameters:
  • joint_id – ID of the joint. This is only used for debug prints.

  • search_distance_limit_rad – Maximum distance the motor moves while searching for the encoder index. Unit: radian.

  • home_offset_rad – Offset from home position to zero position. Unit: radian.

  • profile_step_size_rad – Distance by which the target position of the position profile is changed in each step. Set to a negative value to search for the next encoder index in negative direction. Unit: radian.

HomingReturnCode update_homing()

Perform one step of homing on encoder index.

Searches for the next encoder index in positive direction and, when found, sets it as home position.

Only performs one step, so this method needs to be called in a loop. This method only set the control, one MUST send the control for the motor after calling this method.

The motor is moved with a position profile until either the encoder index is reached or the search distance limit is exceeded. The position is controlled with a simple PD controller.

If the encoder index is found, its position is used as home position. The zero position is offset from the home position by adding the “home

offset” to it (i.e. zero = home pos. + home offset). If the search distance limit is reached before the encoder index occurs, the homing fails.

Returns:

Status of the homing procedure.

double get_distance_travelled_during_homing() const

Get distance between start and end position of homing.

Compute the distance that the joint moved between initialization of homing and reaching the home position.

This can be used to determine the home offset by first moving the joint to the desired zero position, then executing the homing and finally calling this function which will provide the desired home offset.

Returns:

Distance between start and end position of homing.

BlmcJointModule(std::shared_ptr<blmc_drivers::MotorInterface> motor, const double &motor_constant, const double &gear_ratio, const double &zero_angle, const bool &reverse_polarity = false, const double &max_current = 2.1)

Construct a new BlmcJointModule object.

Parameters:
  • motor – is the C++ object allowing us to send commands and receive sensor data.

  • motor_constant – ( \( k \)) is the torque constant of the motor \( \tau_{motor} = k * i_{motor} \)

  • gear_ratio – is the gear ratio between the motor and the joint.

  • zero_angle – is the angle between the closest positive motor index and the zero configuration.

  • reverse_polarity

  • max_current

void set_torque(const double &desired_torque)

Set the joint torque to be sent.

Parameters:

desired_torque – (Nm)

void set_zero_angle(const double &zero_angle)

Set the zero_angle.

The zero_angle is the angle between the closest positive motor index and the zero configuration.

Parameters:

zero_angle – (rad)

void set_joint_polarity(const bool &reverse_polarity)

Define if the motor should turn clock-wize or counter clock-wize.

Parameters:

reverse_polarity – true:reverse rotation axis, false:do nothing.

void send_torque()

send the joint torque to the motor.

The conversion between joint torque and motor current is done automatically.

double get_max_torque() const

Get the maximum admissible joint torque that can be applied.

Returns:

double

double get_sent_torque() const

Get the sent joint torque.

Returns:

double (Nm).

double get_measured_torque() const

Get the measured joint torque.

Returns:

double (Nm).

double get_measured_angle() const

Get the measured angle of the joint.

Returns:

double (rad).

double get_measured_velocity() const

Get the measured velocity of the joint.

This data is computed on board of the control card.

Returns:

double (rad/s).

double get_measured_index_angle() const

Get the measured index angle.

There is one index per motor rotation so there are gear_ratio indexes per joint rotation.

Returns:

double (rad).

double get_zero_angle() const

Get the zero_angle_.

These are the angle between the starting pose and the theoretical zero pose.

Returns:

double (rad).

void set_position_control_gains(double kp, double kd)

Set control gains for PD position controller.

Parameters:
  • kp – P gain ( (Nm) / rad ).

  • kd – D gain ( (Nm) / (rad/s) ).

double execute_position_controller(double target_position_rad) const

Execute one iteration of the position controller.

Parameters:

target_position_rad – Target position (rad).

Returns:

Torque command (Nm).

bool calibrate(double &angle_zero_to_index, double &index_angle, bool mechanical_calibration = false)

This method calibrate the joint position knowing the angle between the closest (in positive torque) motor index and the theoretical zero pose.

Deprecated:

!!!!!!!

Warning, this method should be called in a real time thread!

Param :

void homing_at_current_position(double home_offset_rad)

Set zero position relative to current position.

Parameters:

home_offset_rad – Offset from home position to zero position. Unit: radian.

void init_homing(int joint_id, double search_distance_limit_rad, double home_offset_rad, double profile_step_size_rad = 0.001)

Initialize the homing procedure.

This has to be called before update_homing().

Parameters:
  • joint_id – ID of the joint. This is only used for debug prints.

  • search_distance_limit_rad – Maximum distance the motor moves while searching for the encoder index. Unit: radian.

  • home_offset_rad – Offset from home position to zero position. Unit: radian.

  • profile_step_size_rad – Distance by which the target position of the position profile is changed in each step. Set to a negative value to search for the next encoder index in negative direction. Unit: radian.

HomingReturnCode update_homing()

Perform one step of homing on encoder index.

Searches for the next encoder index in positive direction and, when found, sets it as home position.

Only performs one step, so this method needs to be called in a loop. This method only set the control, one MUST send the control for the motor after calling this method.

The motor is moved with a position profile until either the encoder index is reached or the search distance limit is exceeded. The position is controlled with a simple PD controller.

If the encoder index is found, its position is used as home position. The zero position is offset from the home position by adding the “home

offset” to it (i.e. zero = home pos. + home offset). If the search distance limit is reached before the encoder index occurs, the homing fails.

Returns:

Status of the homing procedure.

double get_distance_travelled_during_homing() const

Get distance between start and end position of homing.

Compute the distance that the joint moved between initialization of homing and reaching the home position.

This can be used to determine the home offset by first moving the joint to the desired zero position, then executing the homing and finally calling this function which will provide the desired home offset.

Returns:

Distance between start and end position of homing.

Private Functions

double joint_torque_to_motor_current(double torque) const

Convert from joint torque to motor current.

Parameters:

torque[in] is the input joint

Returns:

double the equivalent motor current.

double motor_current_to_joint_torque(double current) const

Convert from motor current to joint torque.

Parameters:

current – is the motor current.

Returns:

double is the equivalent joint torque.

double get_motor_measurement(const mi &measurement_id) const

Get motor measurements and check if there are data or not.

Parameters:

measurement_id – is the id of the measurement you want to get. check: blmc_drivers::MotorInterface::MeasurementIndex

Returns:

double the measurement.

long int get_motor_measurement_index(const mi &measurement_id) const

Get the last motor measurement index for a specific data.

If there was no data yet, return NaN

Parameters:

measurement_id – is the id of the measurement you want to get. check: blmc_drivers::MotorInterface::MeasurementIndex

Returns:

double the measurement.

double joint_torque_to_motor_current(double torque) const

Convert from joint torque to motor current.

Parameters:

torque[in] is the input joint

Returns:

double the equivalent motor current.

double motor_current_to_joint_torque(double current) const

Convert from motor current to joint torque.

Parameters:

current – is the motor current.

Returns:

double is the equivalent joint torque.

double get_motor_measurement(const mi &measurement_id) const

Get motor measurements and check if there are data or not.

Parameters:

measurement_id – is the id of the measurement you want to get. check: blmc_drivers::MotorInterface::MeasurementIndex

Returns:

double the measurement.

long int get_motor_measurement_index(const mi &measurement_id) const

Get the last motor measurement index for a specific data.

If there was no data yet, return NaN

Parameters:

measurement_id – is the id of the measurement you want to get. check: blmc_drivers::MotorInterface::MeasurementIndex

Returns:

double the measurement.

Private Members

std::shared_ptr<blmc_drivers::MotorInterface> motor_

This is the pointer to the motor interface.

double motor_constant_

This is the torque constant of the motor: \( \tau_{motor} = k * i_{motor} \).

double gear_ratio_

This correspond to the reduction ( \( \beta \)) between the motor rotation and the joint.

\( \theta_{joint} = \theta_{motor} / \beta \)

double zero_angle_

This is the distance between the closest positive index and the zero configuration.

double polarity_

This change the motor rotation direction.

double max_current_

This is the maximum current we can apply during one experiment.

The program shut down if this value is achieved.

double position_control_gain_p_

P gain of the position PD controller.

double position_control_gain_d_

D gain of the position PD controller.

struct HomingState homing_state_
template<int COUNT>
class BlmcJointModules

This class defines an interface to a collection of BLMC joints.

It creates a BLMCJointModule for every blmc_driver::MotorInterface provided.

Template Parameters:

COUNT

Public Types

typedef Eigen::Matrix<double, COUNT, 1> Vector

Defines a static Eigen vector type in order to define the interface.

typedef Eigen::Matrix<double, COUNT, 1> Vector

Defines a static Eigen vector type in order to define the interface.

Public Functions

inline BlmcJointModules(const std::array<std::shared_ptr<blmc_drivers::MotorInterface>, COUNT> &motors, const Vector &motor_constants, const Vector &gear_ratios, const Vector &zero_angles, const Vector &max_currents)

Construct a new BlmcJointModules object.

Parameters:
  • motors

  • motor_constants

  • gear_ratios

  • zero_angles

inline BlmcJointModules()

Construct a new BlmcJointModules object.

inline void set_motor_array(const std::array<std::shared_ptr<blmc_drivers::MotorInterface>, COUNT> &motors, const Vector &motor_constants, const Vector &gear_ratios, const Vector &zero_angles, const Vector &max_currents)

Set the motor array, by creating the corresponding modules.

Parameters:
  • motors

  • motor_constants

  • gear_ratios

  • zero_angles

inline void send_torques()

Send the registered torques to all modules.

inline void set_joint_polarities(std::array<bool, COUNT> reverse_polarities)

Set the polarities of the joints (see BlmcJointModule::set_joint_polarity)

Parameters:

reverse_polarity

inline void set_torques(const Vector &desired_torques)

Register the joint torques to be sent for all modules.

Parameters:

desired_torques – (Nm)

inline Vector get_max_torques()

Get the maximum admissible joint torque that can be applied.

Returns:

Vector (N/m)

inline Vector get_sent_torques() const

Get the previously sent torques.

Returns:

Vector (Nm)

inline Vector get_measured_torques() const

Get the measured joint torques.

Returns:

Vector (Nm)

inline Vector get_measured_angles() const

Get the measured joint angles.

Returns:

Vector (rad)

inline Vector get_measured_velocities() const

Get the measured joint velocities.

Returns:

Vector (rad/s)

inline void set_zero_angles(const Vector &zero_angles)

Set the zero_angles.

These are the joint angles between the starting pose and the zero theoretical pose of the urdf.

Parameters:

zero_angles – (rad)

inline Vector get_zero_angles() const

Get the zero_angles.

These are the joint angles between the starting pose and the zero theoretical pose of the urdf.

Returns:

Vector (rad)

inline Vector get_measured_index_angles() const

Get the index_angles.

There is one index per motor rotation so there are gear_ratio indexes per joint rotation.

Returns:

Vector (rad)

inline void set_position_control_gains(size_t joint_id, double kp, double kd)

Set position control gains for the specified joint.

Parameters:
  • joint_id – ID of the joint (in range [0, COUNT)).

  • kp – P gain.

  • kd – D gain.

inline void set_position_control_gains(Vector kp, Vector kd)

Set position control gains for all joints.

Parameters:
  • kp – P gains.

  • kd – D gains.

inline HomingReturnCode execute_homing_at_current_position(Vector home_offset_rad)

Perform homing for all joints at endstops.

See BlmcJointModule::homing_at_current_position for description of the arguments.

Returns:

Final status of the homing procedure (since homing happens at current,position, procedure always returns success).

inline HomingReturnCode execute_homing(double search_distance_limit_rad, Vector home_offset_rad, Vector profile_step_size_rad = Vector::Constant(0.001))

Perform homing for all joints.

If one of the joints fails, the complete homing fails. Otherwise it loops until all joints finished. If a joint is finished while others are still running, it is held at the home position.

See BlmcJointModule::update_homing for details on the homing procedure.

See BlmcJointModule::init_homing for description of the arguments.

Returns:

Final status of the homing procedure (either SUCCESS if all joints succeeded or the return code of the first joint that failed).

inline Vector get_distance_travelled_during_homing() const

See also

BlmcJointModule::get_distance_travelled_during_homing

inline GoToReturnCode go_to(Vector angle_to_reach_rad, double average_speed_rad_per_sec = 1.0)

Allow the robot to go to a desired pose.

Once the control done 0 torques is sent.

Parameters:
  • angle_to_reach_rad – (rad)

  • average_speed_rad_per_sec – (rad/sec)

Returns:

GoToReturnCode

inline BlmcJointModules(const std::array<std::shared_ptr<blmc_drivers::MotorInterface>, COUNT> &motors, const Vector &motor_constants, const Vector &gear_ratios, const Vector &zero_angles, const Vector &max_currents)

Construct a new BlmcJointModules object.

Parameters:
  • motors

  • motor_constants

  • gear_ratios

  • zero_angles

inline BlmcJointModules()

Construct a new BlmcJointModules object.

inline void set_motor_array(const std::array<std::shared_ptr<blmc_drivers::MotorInterface>, COUNT> &motors, const Vector &motor_constants, const Vector &gear_ratios, const Vector &zero_angles, const Vector &max_currents)

Set the motor array, by creating the corresponding modules.

Parameters:
  • motors

  • motor_constants

  • gear_ratios

  • zero_angles

inline void send_torques()

Send the registered torques to all modules.

inline void set_joint_polarities(std::array<bool, COUNT> reverse_polarities)

Set the polarities of the joints (see BlmcJointModule::set_joint_polarity)

Parameters:

reverse_polarity

inline void set_torques(const Vector &desired_torques)

Register the joint torques to be sent for all modules.

Parameters:

desired_torques – (Nm)

inline Vector get_max_torques()

Get the maximum admissible joint torque that can be applied.

Returns:

Vector (N/m)

inline Vector get_sent_torques() const

Get the previously sent torques.

Returns:

Vector (Nm)

inline Vector get_measured_torques() const

Get the measured joint torques.

Returns:

Vector (Nm)

inline Vector get_measured_angles() const

Get the measured joint angles.

Returns:

Vector (rad)

inline Vector get_measured_velocities() const

Get the measured joint velocities.

Returns:

Vector (rad/s)

inline void set_zero_angles(const Vector &zero_angles)

Set the zero_angles.

These are the joint angles between the starting pose and the zero theoretical pose of the urdf.

Parameters:

zero_angles – (rad)

inline Vector get_zero_angles() const

Get the zero_angles.

These are the joint angles between the starting pose and the zero theoretical pose of the urdf.

Returns:

Vector (rad)

inline Vector get_measured_index_angles() const

Get the index_angles.

There is one index per motor rotation so there are gear_ratio indexes per joint rotation.

Returns:

Vector (rad)

inline void set_position_control_gains(size_t joint_id, double kp, double kd)

Set position control gains for the specified joint.

Parameters:
  • joint_id – ID of the joint (in range [0, COUNT)).

  • kp – P gain.

  • kd – D gain.

inline void set_position_control_gains(Vector kp, Vector kd)

Set position control gains for all joints.

Parameters:
  • kp – P gains.

  • kd – D gains.

inline HomingReturnCode execute_homing_at_current_position(Vector home_offset_rad)

Perform homing for all joints at endstops.

See BlmcJointModule::homing_at_current_position for description of the arguments.

Returns:

Final status of the homing procedure (since homing happens at current,position, procedure always returns success).

inline HomingReturnCode execute_homing(double search_distance_limit_rad, Vector home_offset_rad, Vector profile_step_size_rad = Vector::Constant(0.001))

Perform homing for all joints.

If one of the joints fails, the complete homing fails. Otherwise it loops until all joints finished. If a joint is finished while others are still running, it is held at the home position.

See BlmcJointModule::update_homing for details on the homing procedure.

See BlmcJointModule::init_homing for description of the arguments.

Returns:

Final status of the homing procedure (either SUCCESS if all joints succeeded or the return code of the first joint that failed).

inline Vector get_distance_travelled_during_homing() const

See also

BlmcJointModule::get_distance_travelled_during_homing

inline GoToReturnCode go_to(Vector angle_to_reach_rad, double average_speed_rad_per_sec = 1.0)

Allow the robot to go to a desired pose.

Once the control done 0 torques is sent.

Parameters:
  • angle_to_reach_rad – (rad)

  • average_speed_rad_per_sec – (rad/sec)

Returns:

GoToReturnCode

Private Members

std::array<std::shared_ptr<BlmcJointModule>, COUNT> modules_

These are the BLMCJointModule objects corresponding to a robot.

class CanBus : public blmc_drivers::CanBusInterface, public blmc_drivers::CanBusInterface
#include <can_bus.hpp>

CanBus is the implementation of the CanBusInterface.

Public Functions

CanBus(const std::string &can_interface_name, const size_t &history_length = 1000)

Construct a new CanBus object.

Parameters:
  • can_interface_name

  • history_length

virtual ~CanBus()

Destroy the CanBus object.

inline virtual std::shared_ptr<const CanframeTimeseries> get_output_frame() const

Getters.

Get the output frame

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual std::shared_ptr<const CanframeTimeseries> get_input_frame()

Get the input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual std::shared_ptr<const CanframeTimeseries> get_sent_input_frame()

Get the input frame thas has been sent.

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual void set_input_frame(const CanBusFrame &input_frame)

Setters.

Set the input frame

Parameters:

input_frame

virtual void send_if_input_changed()

Sender.

Send the queue of message to the can network

CanBus(const std::string &can_interface_name, const size_t &history_length = 1000)

Construct a new CanBus object.

Parameters:
  • can_interface_name

  • history_length

virtual ~CanBus()

Destroy the CanBus object.

inline virtual std::shared_ptr<const CanframeTimeseries> get_output_frame() const

Getters.

Get the output frame

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual std::shared_ptr<const CanframeTimeseries> get_input_frame()

Get the input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual std::shared_ptr<const CanframeTimeseries> get_sent_input_frame()

Get the input frame thas has been sent.

Returns:

std::shared_ptr<const CanframeTimeseries>

inline virtual void set_input_frame(const CanBusFrame &input_frame)

Setters.

Set the input frame

Parameters:

input_frame

virtual void send_if_input_changed()

Sender.

Send the queue of message to the can network

Private Functions

void loop()

Execute the communication loop with the can bus.

void send_frame(const CanBusFrame &unstamped_can_frame)

Send input data.

Parameters:

unstamped_can_frame – is a frame without id nor time.

CanBusFrame receive_frame()

Get the output frame from the bus.

Returns:

CanBusFrame is the output frame data.

CanBusConnection setup_can(std::string name, uint32_t err_mask)

Setup and initialize the CanBus object.

It connects to the can bus. This method is used once in the constructor.

Parameters:
  • name – is the can card name.

  • err_mask, always – used with “0” so far (TODO: Manuel explain)

Returns:

CanBusConnection

void loop()

Execute the communication loop with the can bus.

void send_frame(const CanBusFrame &unstamped_can_frame)

Send input data.

Parameters:

unstamped_can_frame – is a frame without id nor time.

CanBusFrame receive_frame()

Get the output frame from the bus.

Returns:

CanBusFrame is the output frame data.

CanBusConnection setup_can(std::string name, uint32_t err_mask)

Setup and initialize the CanBus object.

It connects to the can bus. This method is used once in the constructor.

Parameters:
  • name – is the can card name.

  • err_mask, always – used with “0” so far (TODO: Manuel explain)

Returns:

CanBusConnection

Private Members

real_time_tools::SingletypeThreadsafeObject<CanBusConnection, 1> can_connection_

Attributes.

can_connection_ is the communication object allowing to send or receive can frames.

std::shared_ptr<time_series::TimeSeries<CanBusFrame>> input_

input_ is a list of time stamped frame to be send to the can network.

std::shared_ptr<time_series::TimeSeries<CanBusFrame>> sent_input_

sent_inupt_ is the list of the input already sent to the network.

std::shared_ptr<time_series::TimeSeries<CanBusFrame>> output_

output_ is the list of the frames received from the can network.

bool is_loop_active_

This boolean makes sure that the loop is not active upon destruction of the current object.

real_time_tools::RealTimeThread rt_thread_

rt_thread_ is the thread object allowing us to spawn real-time threads.

std::string log_dir_

Log directory.

std::string name_

time_log_name is the name of the loggin

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

private attributes and methods

This function is an helper that allows us to launch real-time thread in xenaomai, ubunt, or rt-preempt seemlessly.

Parameters:

instance_pointer

Returns:

THREAD_FUNCTION_RETURN_TYPE (is void or void* depending on the OS.

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

private attributes and methods

This function is an helper that allows us to launch real-time thread in xenaomai, ubunt, or rt-preempt seemlessly.

Parameters:

instance_pointer

Returns:

THREAD_FUNCTION_RETURN_TYPE (is void or void* depending on the OS.

class CanBusConnection
#include <can_bus.hpp>

CanBusConnection is a data structure that contains the hardware details for the connection between to can cards.

Public Members

struct sockaddr_can send_addr

send_addr is the ip address where to send the the messages.

int socket

socket is the port through which the messages will be processed

class CanBusFrame
#include <can_bus.hpp>

CanBusFrame is a class that contains a fixed sized amount of data to be send or received via the can bus.

Public Functions

inline void print() const
inline void print() const

Public Members

std::array<uint8_t, 8> data

data is the acutal data to be sent/received.

uint8_t dlc

dlc is the size of the message.

can_id_t id

id is the id number return by the CAN bus.

class CanBusInterface : public blmc_drivers::DeviceInterface, public blmc_drivers::DeviceInterface
#include <can_bus.hpp>

CanBusInterface is an abstract class that defines an API for the communication via Can bus.

Subclassed by blmc_drivers::CanBus, blmc_drivers::CanBus

Public Types

typedef time_series::TimeSeries<CanBusFrame> CanframeTimeseries

CanframeTimeseries is a simple sohortcut.

typedef time_series::TimeSeries<CanBusFrame> CanframeTimeseries

CanframeTimeseries is a simple sohortcut.

Public Functions

inline virtual ~CanBusInterface()

Destroy the CanBusInterface object.

virtual std::shared_ptr<const CanframeTimeseries> get_output_frame() const = 0

getters

Get the output frame

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual std::shared_ptr<const CanframeTimeseries> get_input_frame() = 0

Get the input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual std::shared_ptr<const CanframeTimeseries> get_sent_input_frame() = 0

Get the sent input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual void set_input_frame(const CanBusFrame &input_frame) = 0

setters

Set the input frame saves the input frame to be sent in a queue.

Parameters:

input_frame

virtual void send_if_input_changed() = 0

Sender.

send all the input frame to the can network

inline virtual ~CanBusInterface()

Destroy the CanBusInterface object.

virtual std::shared_ptr<const CanframeTimeseries> get_output_frame() const = 0

getters

Get the output frame

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual std::shared_ptr<const CanframeTimeseries> get_input_frame() = 0

Get the input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual std::shared_ptr<const CanframeTimeseries> get_sent_input_frame() = 0

Get the sent input frame.

Returns:

std::shared_ptr<const CanframeTimeseries>

virtual void set_input_frame(const CanBusFrame &input_frame) = 0

setters

Set the input frame saves the input frame to be sent in a queue.

Parameters:

input_frame

virtual void send_if_input_changed() = 0

Sender.

send all the input frame to the can network

class CanBusMotorBoard : public blmc_drivers::MotorBoardInterface, public blmc_drivers::MotorBoardInterface
#include <motor_board.hpp>

This class CanBusMotorBoard implements a MotorBoardInterface specific to CAN networks.

Public Functions

CanBusMotorBoard(std::shared_ptr<CanBusInterface> can_bus, const size_t &history_length = 1000, const int &control_timeout_ms = 100)

Construct a new CanBusMotorBoard object.

Parameters:
  • can_bus

  • history_length

~CanBusMotorBoard()

Destroy the CanBusMotorBoard object.

inline virtual Ptr<const ScalarTimeseries> get_measurement(const int &index) const

Getters.

Get the measurement data.

Parameters:

index – is the kind of measurement we are insterested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the last measurements acquiered from the CAN card.

inline virtual Ptr<const StatusTimeseries> get_status() const

Get the status of the CAN card.

Returns:

Ptr<const StatusTimeseries> is the list of last acquiered status.

inline virtual Ptr<const ScalarTimeseries> get_control(const int &index) const

Get the controls to be sent.

Parameters:

index – the kind of control we are interested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the control to be sent.

inline virtual Ptr<const CommandTimeseries> get_command() const

Get the commands to be sent.

Returns:

Ptr<const CommandTimeseries> is the list of the command to be sent.

inline virtual Ptr<const ScalarTimeseries> get_sent_control(const int &index) const

Get the already sent controls.

Parameters:

index – the kind of control we are interested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the sent cotnrols.

inline virtual Ptr<const CommandTimeseries> get_sent_command() const

Get the already sent commands.

Returns:

Ptr<const CommandTimeseries> is the list of the sent cotnrols.

inline virtual void set_control(const double &control, const int &index)

Setters.

Set the controls, see MotorBoardInterface::set_control

Parameters:
  • control

  • index

inline virtual void set_command(const MotorBoardCommand &command)

Set the commands, see MotorBoardInterface::set_command.

Parameters:

command

virtual void send_if_input_changed()

Send the actual command and controls.

void wait_until_ready()

returns only once board and motors are ready.

bool is_ready()
void pause_motors()

Todo:

: this function should go away, and we should add somewhere a warning in case there is a timeout

void disable_can_recv_timeout()

Disable the can reciever timeout.

CanBusMotorBoard(std::shared_ptr<CanBusInterface> can_bus, const size_t &history_length = 1000, const int &control_timeout_ms = 100)

Construct a new CanBusMotorBoard object.

Parameters:
  • can_bus

  • history_length

~CanBusMotorBoard()

Destroy the CanBusMotorBoard object.

inline virtual Ptr<const ScalarTimeseries> get_measurement(const int &index) const

Getters.

Get the measurement data.

Parameters:

index – is the kind of measurement we are insterested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the last measurements acquiered from the CAN card.

inline virtual Ptr<const StatusTimeseries> get_status() const

Get the status of the CAN card.

Returns:

Ptr<const StatusTimeseries> is the list of last acquiered status.

inline virtual Ptr<const ScalarTimeseries> get_control(const int &index) const

Get the controls to be sent.

Parameters:

index – the kind of control we are interested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the control to be sent.

inline virtual Ptr<const CommandTimeseries> get_command() const

Get the commands to be sent.

Returns:

Ptr<const CommandTimeseries> is the list of the command to be sent.

inline virtual Ptr<const ScalarTimeseries> get_sent_control(const int &index) const

Get the already sent controls.

Parameters:

index – the kind of control we are interested in.

Returns:

Ptr<const ScalarTimeseries> is the list of the sent cotnrols.

inline virtual Ptr<const CommandTimeseries> get_sent_command() const

Get the already sent commands.

Returns:

Ptr<const CommandTimeseries> is the list of the sent cotnrols.

inline virtual void set_control(const double &control, const int &index)

Setters.

Set the controls, see MotorBoardInterface::set_control

Parameters:
  • control

  • index

inline virtual void set_command(const MotorBoardCommand &command)

Set the commands, see MotorBoardInterface::set_command.

Parameters:

command

virtual void send_if_input_changed()

Send the actual command and controls.

void wait_until_ready()

returns only once board and motors are ready.

bool is_ready()
void pause_motors()

Todo:

: this function should go away, and we should add somewhere a warning in case there is a timeout

void disable_can_recv_timeout()

Disable the can reciever timeout.

Private Types

enum CanframeIDs

These are the frame IDs that define the kind of data we acquiere from the CAN bus.

Values:

enumerator COMMAND_ID
enumerator IqRef
enumerator STATUSMSG
enumerator Iq
enumerator POS
enumerator SPEED
enumerator ADC6
enumerator ENC_INDEX
enumerator COMMAND_ID
enumerator IqRef
enumerator STATUSMSG
enumerator Iq
enumerator POS
enumerator SPEED
enumerator ADC6
enumerator ENC_INDEX
enum CanframeIDs

These are the frame IDs that define the kind of data we acquiere from the CAN bus.

Values:

enumerator COMMAND_ID
enumerator IqRef
enumerator STATUSMSG
enumerator Iq
enumerator POS
enumerator SPEED
enumerator ADC6
enumerator ENC_INDEX
enumerator COMMAND_ID
enumerator IqRef
enumerator STATUSMSG
enumerator Iq
enumerator POS
enumerator SPEED
enumerator ADC6
enumerator ENC_INDEX

Private Functions

template<typename T>
inline int32_t bytes_to_int32(T bytes)

private methods ========================================================

Useful converters

Converts from bytes to int32.

Template Parameters:

T – this is the type of the bytes convert.

Parameters:

bytes – The bytes value

Returns:

int32_t the output integer in int32.

inline float q24_to_float(int32_t qval)

Convert from 24-bit normalized fixed-point to float.

Parameters:

qval – is the floating base point.

Returns:

float is the converted value

inline int32_t float_to_q24(float fval)

Converts from float to 24-bit normalized fixed-point.

Parameters:

fval

Returns:

int32_t

template<typename T>
inline float qbytes_to_float(T qbytes)

Converts from qbytes to float.

Template Parameters:

T – the type of byte to manage

Parameters:

qbytes – the input value in bytes

Returns:

float the output value.

void send_newest_controls()

send the controls to the cards.

Parameters:

controls – are the controls to be sent.

void send_newest_command()

send the latest commands to the cards.

void loop()

Is the loop that constently communicate with the network.

void print_status()

Display details of this object.

template<typename T>
inline int32_t bytes_to_int32(T bytes)

private methods ========================================================

Useful converters

Converts from bytes to int32.

Template Parameters:

T – this is the type of the bytes convert.

Parameters:

bytes – The bytes value

Returns:

int32_t the output integer in int32.

inline float q24_to_float(int32_t qval)

Convert from 24-bit normalized fixed-point to float.

Parameters:

qval – is the floating base point.

Returns:

float is the converted value

inline int32_t float_to_q24(float fval)

Converts from float to 24-bit normalized fixed-point.

Parameters:

fval

Returns:

int32_t

template<typename T>
inline float qbytes_to_float(T qbytes)

Converts from qbytes to float.

Template Parameters:

T – the type of byte to manage

Parameters:

qbytes – the input value in bytes

Returns:

float the output value.

void send_newest_controls()

send the controls to the cards.

Parameters:

controls – are the controls to be sent.

void send_newest_command()

send the latest commands to the cards.

void loop()

Is the loop that constently communicate with the network.

void print_status()

Display details of this object.

Private Members

std::shared_ptr<CanBusInterface> can_bus_

This is the pointer to the can bus to communicate with.

Vector<Ptr<ScalarTimeseries>> measurement_

Outputs.

measurement_ contains all the measurements acquiered from the CAN board.

Ptr<StatusTimeseries> status_

This is the status history of the CAN board.

Vector<Ptr<ScalarTimeseries>> control_

Inputs.

This is the buffer of the controls to be sent to card.

Ptr<CommandTimeseries> command_

This is the buffer of the commands to be sent to the card.

Vector<Ptr<ScalarTimeseries>> sent_control_

Log.

This is the history of the already sent controls.

Ptr<CommandTimeseries> sent_command_

This is the history of the already sent commands.

bool is_loop_active_

Loop management.

This boolean makes sure that the loop is stopped upon destruction of this object.

bool motors_are_paused_

Are motor in idle mode = 0 torques? @TODO update this documentation with the actual behavior.

int control_timeout_ms_

If no control is sent for more than control_timeout_ms_ the board will shut down.

real_time_tools::RealTimeThread rt_thread_

This is the thread object that allow to spwan a real-time thread or not dependening on the current OS.

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

This is the helper function used for spawning the real time thread.

Parameters:

instance_pointer – is the current object in this case.

Returns:

THREAD_FUNCTION_RETURN_TYPE depends on the current OS.

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

This is the helper function used for spawning the real time thread.

Parameters:

instance_pointer – is the current object in this case.

Returns:

THREAD_FUNCTION_RETURN_TYPE depends on the current OS.

class ConstTorqueControl

This is a basic PD controller to be used in the demos of this package.

Public Functions

inline ConstTorqueControl(std::vector<SafeMotor_ptr> motor_list)

Construct a new ConstTorqueControl object.

Parameters:

motor_slider_pairs

inline ~ConstTorqueControl()

Destroy the ConstTorqueControl object.

inline void start_loop()

This method is a helper to start the thread loop.

void stop_loop()

Stop the control and dump the data.

Private Functions

void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::vector<SafeMotor_ptr> motor_list_

This is list of motors.

real_time_tools::RealTimeThread rt_thread_

This is the real time thread object.

bool stop_loop_

managing the stopping of the loop

unsigned memory_buffer_size_

memory_buffer_size_ is the max size of the memory buffer.

std::vector<std::deque<double>> encoders_

Encoder data.

std::vector<std::deque<double>> velocities_

Velocity data.

std::vector<std::deque<double>> currents_

current data

std::vector<std::deque<double>> control_buffer_

control_buffer_

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

class Controller

This is a simple PD control on one motor and one slider.

Public Functions

inline Controller(std::shared_ptr<blmc_drivers::Motor> motor, std::shared_ptr<blmc_drivers::AnalogSensor> analog_sensor)

Construct a new Controller object.

Parameters:
  • motor

  • analog_sensor

inline void start_loop()

main control loop

Public Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

Private Functions

inline void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::shared_ptr<blmc_drivers::Motor> motor_

one motor.

std::shared_ptr<blmc_drivers::AnalogSensor> analog_sensor_

one slider.

real_time_tools::RealTimeThread rt_thread_

the realt time thread object.

class DeviceInterface

this class exists purely for logical reasons, it does not in itself implement anything.

the purpose of this class is to provide guidelines how a device should be implemented. any device has a number of inputs and outputs, see the following diagram for an example with two inputs and outputs.

generally, we expect the following functions to be implemented:

  • a set function for each input (several inputs may share a set function which takes an index argument).

  • a send_if_input_changed() function which will send the inputs to the device if any of them have changed.

  • functions to access the current inputs and outputs, as well as the inputs which have been sent to the device. Rather than just returning the latest elements, these function should return a time series of these objects, such that the user can synchronize (e.g. wait for the next element or step through them one by one such that none of them is missed)

_images/device_class_diagram.svg

Subclassed by blmc_drivers::AnalogSensorInterface, blmc_drivers::AnalogSensorInterface, blmc_drivers::CanBusInterface, blmc_drivers::CanBusInterface, blmc_drivers::LegInterface, blmc_drivers::LegInterface, blmc_drivers::MotorBoardInterface, blmc_drivers::MotorBoardInterface, blmc_drivers::MotorInterface, blmc_drivers::MotorInterface

struct Hardware

Public Members

std::shared_ptr<blmc_drivers::CanBusInterface> can_bus
std::shared_ptr<blmc_drivers::MotorBoardInterface> motor_board
std::shared_ptr<blmc_drivers::MotorInterface> motor
std::shared_ptr<blmc_drivers::AnalogSensorInterface> slider
struct HomingState

State variables required for the homing.

Public Members

int joint_id = 0

Id of the joint. Just used for debug prints.

double search_distance_limit_rad = 0.0

Max. distance to move while searching the encoder index.

double home_offset_rad = 0.0

Offset from home position to zero position.

double profile_step_size_rad = 0.0

Step size for the position profile.

long int last_encoder_index_time_index = 0

Timestamp from when the encoder index was seen the last time.

uint32_t step_count = 0

Number of profile steps already taken.

double target_position_rad = 0.0

Current target position of the position profile.

HomingReturnCode status = HomingReturnCode::NOT_INITIALIZED

Current status of the homing procedure.

double start_position

Position at which homing is started.

double end_position

Position at which homing is ended (before resetting position).

This is only set when status is SUCCEEDED. Together with start_position it can be used to determine the distance the joint travelled during the homing procedure (e.g. useful for home offset calibration).

class Leg : public blmc_drivers::LegInterface, public blmc_drivers::LegInterface
#include <leg.hpp>

The leg class is the implementation of the LegInterface.

This is the decalartion and the definition of the class as it is very simple.

Public Functions

inline Leg(std::shared_ptr<MotorInterface> hip_motor, std::shared_ptr<MotorInterface> knee_motor)

Construct a new Leg object.

Parameters:
  • hip_motor – is the pointer to the hip motor

  • knee_motor – is the pointer to the knee motor

inline virtual ~Leg()

Destroy the Leg object.

inline virtual Ptr<const ScalarTimeseries> get_motor_measurement(const int &motor_index, const int &measurement_index) const

Getters.

Get the motor measurements.

Parameters:
  • motor_index

  • measurement_index

Returns:

Ptr<const ScalarTimeseries>

inline virtual Ptr<const ScalarTimeseries> get_current_target(const int &motor_index) const

Get the actual target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

inline virtual Ptr<const ScalarTimeseries> get_sent_current_target(const int &motor_index) const

Get the last sent target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

inline virtual void set_current_target(const double &current_target, const int &motor_index)

setters ================================================================

inline virtual void send_if_input_changed()

sender =================================================================

inline Leg(std::shared_ptr<MotorInterface> hip_motor, std::shared_ptr<MotorInterface> knee_motor)

Construct a new Leg object.

Parameters:
  • hip_motor – is the pointer to the hip motor

  • knee_motor – is the pointer to the knee motor

inline virtual ~Leg()

Destroy the Leg object.

inline virtual Ptr<const ScalarTimeseries> get_motor_measurement(const int &motor_index, const int &measurement_index) const

Getters.

Get the motor measurements.

Parameters:
  • motor_index

  • measurement_index

Returns:

Ptr<const ScalarTimeseries>

inline virtual Ptr<const ScalarTimeseries> get_current_target(const int &motor_index) const

Get the actual target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

inline virtual Ptr<const ScalarTimeseries> get_sent_current_target(const int &motor_index) const

Get the last sent target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

inline virtual void set_current_target(const double &current_target, const int &motor_index)

setters ================================================================

inline virtual void send_if_input_changed()

sender =================================================================

Private Members

std::array<std::shared_ptr<MotorInterface>, 2> motors_

========================================================================

This list contains pointers to two motors. This motors are respectively the hip and the knee of the leg.

class LegController

Simple PD control on the leg.

Public Functions

inline LegController(std::shared_ptr<blmc_drivers::Leg> leg, std::shared_ptr<blmc_drivers::AnalogSensor> analog_sensor)

Construct a new LegController object.

Parameters:
  • leg

  • analog_sensor

inline ~LegController()

Destroy the LegController object.

inline void start_loop()

helper to strat the real time thread.

Public Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

Private Functions

inline void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::shared_ptr<blmc_drivers::Leg> leg_

is the leg to control

std::shared_ptr<blmc_drivers::AnalogSensor> analog_sensor_

is the list of sliders

real_time_tools::RealTimeThread rt_thread_

is the real time thread object.

bool stop_loop_

manages the shutdown of the controller

class LegInterface : public blmc_drivers::DeviceInterface, public blmc_drivers::DeviceInterface
#include <leg.hpp>

This class defines an interface to control a leg.

This legg is composed of 2 motor, one for the hip and one for the knee.

Subclassed by blmc_drivers::Leg, blmc_drivers::Leg

Public Types

enum MotorMeasurementIndexing

MotorMeasurementIndexing this enum allow to access the different kind of sensor measurements in an understandable way in the code.

Values:

enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator motor_measurement_count
enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator motor_measurement_count
enum MotorIndexing

This enum list the motors in the leg.

Values:

enumerator hip
enumerator knee
enumerator motor_count
enumerator hip
enumerator knee
enumerator motor_count
enum MotorMeasurementIndexing

MotorMeasurementIndexing this enum allow to access the different kind of sensor measurements in an understandable way in the code.

Values:

enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator motor_measurement_count
enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator motor_measurement_count
enum MotorIndexing

This enum list the motors in the leg.

Values:

enumerator hip
enumerator knee
enumerator motor_count
enumerator hip
enumerator knee
enumerator motor_count
typedef time_series::TimeSeries<double> ScalarTimeseries

ScalarTimeseries is a simple shortcut for more intelligible code.

template<typename Type>
using Ptr = std::shared_ptr<Type>

This is a shortcut for creating shared pointer in a simpler writting expression.

Template Parameters:

Type – is the template paramer of the shared pointer.

typedef time_series::TimeSeries<double> ScalarTimeseries

ScalarTimeseries is a simple shortcut for more intelligible code.

template<typename Type>
using Ptr = std::shared_ptr<Type>

This is a shortcut for creating shared pointer in a simpler writting expression.

Template Parameters:

Type – is the template paramer of the shared pointer.

Public Functions

inline virtual ~LegInterface()

Destroy the LegInterface object.

virtual Ptr<const ScalarTimeseries> get_motor_measurement(const int &motor_index, const int &measurement_index) const = 0

Getters.

Get the device output

Parameters:
  • motor_index[in] designate the motor from which we want the data from.

  • measurement_index[in] is teh kind of data we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual Ptr<const ScalarTimeseries> get_current_target(const int &motor_index) const = 0

Get the actual target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual Ptr<const ScalarTimeseries> get_sent_current_target(const int &motor_index) const = 0

Get the last sent target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual void set_current_target(const double &current_target, const int &motor_index) = 0

Setters.

Set the current target saves internally the desired current. This data is not send to the motor yet. Please call send_if_input_changed in order to actually send the data to the card.

Parameters:
  • current_target – is the current to achieve on the motor card.

  • motor_index – is the motor to control.

virtual void send_if_input_changed() = 0

Sender.

Actually send the target current to the motor cards.

inline virtual ~LegInterface()

Destroy the LegInterface object.

virtual Ptr<const ScalarTimeseries> get_motor_measurement(const int &motor_index, const int &measurement_index) const = 0

Getters.

Get the device output

Parameters:
  • motor_index[in] designate the motor from which we want the data from.

  • measurement_index[in] is teh kind of data we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual Ptr<const ScalarTimeseries> get_current_target(const int &motor_index) const = 0

Get the actual target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual Ptr<const ScalarTimeseries> get_sent_current_target(const int &motor_index) const = 0

Get the last sent target current.

Parameters:

motor_index[in] designate the motor from which we want the data from.

Returns:

Ptr<const ScalarTimeseries> is the list of the lasts time stamped acquiered.

virtual void set_current_target(const double &current_target, const int &motor_index) = 0

Setters.

Set the current target saves internally the desired current. This data is not send to the motor yet. Please call send_if_input_changed in order to actually send the data to the card.

Parameters:
  • current_target – is the current to achieve on the motor card.

  • motor_index – is the motor to control.

virtual void send_if_input_changed() = 0

Sender.

Actually send the target current to the motor cards.

class Motor : public blmc_drivers::MotorInterface, public blmc_drivers::MotorInterface
#include <motor.hpp>

This class implements the MotorInterface.

Subclassed by blmc_drivers::SafeMotor, blmc_drivers::SafeMotor

Public Functions

Motor(Ptr<MotorBoardInterface> board, bool motor_id)

Construct a new Motor object.

Parameters:
  • board – is the MotorBoard to be used.

  • motor_id – is the id of the motor on the on-board card

inline virtual ~Motor()

Destroy the Motor object.

inline virtual void send_if_input_changed()

Actually send the command and controls via the network, See MotorInterface for more information.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index = 0) const

Getters.

Get the measurements

Parameters:

index – is the kind of measurement we are instersted in. see MotorInterface::MeasurementIndex.

Returns:

Ptr<const ScalarTimeseries> The history of the measurement

virtual Ptr<const ScalarTimeseries> get_current_target() const

Get the current target to be sent.

Returns:

Ptr<const ScalarTimeseries> the list of current values to be sent.

virtual Ptr<const ScalarTimeseries> get_sent_current_target() const

Get the already sent current target values.

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target)

Setters.

Set the current (Ampere) target. See MotorInterface for more information.

Parameters:

current_target – in Ampere

inline virtual void set_command(const MotorBoardCommand &command)

Set the command.

See MotorInterface for more information.

Parameters:

command

virtual void print() const

Print the motor status and state.

Motor(Ptr<MotorBoardInterface> board, bool motor_id)

Construct a new Motor object.

Parameters:
  • board – is the MotorBoard to be used.

  • motor_id – is the id of the motor on the on-board card

inline virtual ~Motor()

Destroy the Motor object.

inline virtual void send_if_input_changed()

Actually send the command and controls via the network, See MotorInterface for more information.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index = 0) const

Getters.

Get the measurements

Parameters:

index – is the kind of measurement we are instersted in. see MotorInterface::MeasurementIndex.

Returns:

Ptr<const ScalarTimeseries> The history of the measurement

virtual Ptr<const ScalarTimeseries> get_current_target() const

Get the current target to be sent.

Returns:

Ptr<const ScalarTimeseries> the list of current values to be sent.

virtual Ptr<const ScalarTimeseries> get_sent_current_target() const

Get the already sent current target values.

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target)

Setters.

Set the current (Ampere) target. See MotorInterface for more information.

Parameters:

current_target – in Ampere

inline virtual void set_command(const MotorBoardCommand &command)

Set the command.

See MotorInterface for more information.

Parameters:

command

virtual void print() const

Print the motor status and state.

Protected Attributes

Ptr<MotorBoardInterface> board_

The MotorBoard to be used for the communication.

bool motor_id_

The id of the motor on the MotorBoard.

class MotorBoardCommand
#include <motor_board.hpp>

This MotorBoardCommand class is a data structurs that defines a command.

Public Types

enum IDs

IDs are the different implemented commands that one can send to the MotorBoard.

Values:

enumerator ENABLE_SYS
enumerator ENABLE_MTR1
enumerator ENABLE_MTR2
enumerator ENABLE_VSPRING1
enumerator ENABLE_VSPRING2
enumerator SEND_CURRENT
enumerator SEND_POSITION
enumerator SEND_VELOCITY
enumerator SEND_ADC6
enumerator SEND_ENC_INDEX
enumerator SEND_ALL
enumerator SET_CAN_RECV_TIMEOUT
enumerator ENABLE_POS_ROLLOVER_ERROR
enumerator ENABLE_SYS
enumerator ENABLE_MTR1
enumerator ENABLE_MTR2
enumerator ENABLE_VSPRING1
enumerator ENABLE_VSPRING2
enumerator SEND_CURRENT
enumerator SEND_POSITION
enumerator SEND_VELOCITY
enumerator SEND_ADC6
enumerator SEND_ENC_INDEX
enumerator SEND_ALL
enumerator SET_CAN_RECV_TIMEOUT
enumerator ENABLE_POS_ROLLOVER_ERROR
enum Contents

Is the different command status.

Values:

enumerator ENABLE
enumerator DISABLE
enumerator ENABLE
enumerator DISABLE
enum IDs

IDs are the different implemented commands that one can send to the MotorBoard.

Values:

enumerator ENABLE_SYS
enumerator ENABLE_MTR1
enumerator ENABLE_MTR2
enumerator ENABLE_VSPRING1
enumerator ENABLE_VSPRING2
enumerator SEND_CURRENT
enumerator SEND_POSITION
enumerator SEND_VELOCITY
enumerator SEND_ADC6
enumerator SEND_ENC_INDEX
enumerator SEND_ALL
enumerator SET_CAN_RECV_TIMEOUT
enumerator ENABLE_POS_ROLLOVER_ERROR
enumerator ENABLE_SYS
enumerator ENABLE_MTR1
enumerator ENABLE_MTR2
enumerator ENABLE_VSPRING1
enumerator ENABLE_VSPRING2
enumerator SEND_CURRENT
enumerator SEND_POSITION
enumerator SEND_VELOCITY
enumerator SEND_ADC6
enumerator SEND_ENC_INDEX
enumerator SEND_ALL
enumerator SET_CAN_RECV_TIMEOUT
enumerator ENABLE_POS_ROLLOVER_ERROR
enum Contents

Is the different command status.

Values:

enumerator ENABLE
enumerator DISABLE
enumerator ENABLE
enumerator DISABLE

Public Functions

inline MotorBoardCommand()

Construct a new MotorBoardCommand object.

inline MotorBoardCommand(uint32_t id, int32_t content)

Construct a new MotorBoardCommand object.

Parameters:
  • id – defines the command to apply.

  • content – defines of the command is enabled or disabled.

inline void print() const

Display on a terminal the status of the message.

inline MotorBoardCommand()

Construct a new MotorBoardCommand object.

inline MotorBoardCommand(uint32_t id, int32_t content)

Construct a new MotorBoardCommand object.

Parameters:
  • id – defines the command to apply.

  • content – defines of the command is enabled or disabled.

inline void print() const

Display on a terminal the status of the message.

Public Members

uint32_t id_

id_ is the command to be modifies on the card.

int32_t content_

content_ is the value of teh command to be sent to the cards.

class MotorBoardInterface : public blmc_drivers::DeviceInterface, public blmc_drivers::DeviceInterface
#include <motor_board.hpp>

MotorBoardInterface declares an API to inacte with a MotorBoard.

Subclassed by blmc_drivers::CanBusMotorBoard, blmc_drivers::CanBusMotorBoard

Public Types

enum MeasurementIndex

This is the list of the measurement we can access.

Values:

enumerator current_0
enumerator current_1
enumerator position_0
enumerator position_1
enumerator velocity_0
enumerator velocity_1
enumerator analog_0
enumerator analog_1
enumerator encoder_index_0
enumerator encoder_index_1
enumerator measurement_count
enumerator current_0
enumerator current_1
enumerator position_0
enumerator position_1
enumerator velocity_0
enumerator velocity_1
enumerator analog_0
enumerator analog_1
enumerator encoder_index_0
enumerator encoder_index_1
enumerator measurement_count
enum ControlIndex

This is the list of the controls we can send.

Values:

enumerator current_target_0
enumerator current_target_1
enumerator control_count
enumerator current_target_0
enumerator current_target_1
enumerator control_count
enum MeasurementIndex

This is the list of the measurement we can access.

Values:

enumerator current_0
enumerator current_1
enumerator position_0
enumerator position_1
enumerator velocity_0
enumerator velocity_1
enumerator analog_0
enumerator analog_1
enumerator encoder_index_0
enumerator encoder_index_1
enumerator measurement_count
enumerator current_0
enumerator current_1
enumerator position_0
enumerator position_1
enumerator velocity_0
enumerator velocity_1
enumerator analog_0
enumerator analog_1
enumerator encoder_index_0
enumerator encoder_index_1
enumerator measurement_count
enum ControlIndex

This is the list of the controls we can send.

Values:

enumerator current_target_0
enumerator current_target_1
enumerator control_count
enumerator current_target_0
enumerator current_target_1
enumerator control_count
typedef time_series::TimeSeries<double> ScalarTimeseries

A useful shortcut.

typedef time_series::Index Index

A useful shortcut.

typedef time_series::TimeSeries<Index> IndexTimeseries

A useful shortcut.

typedef time_series::TimeSeries<MotorBoardStatus> StatusTimeseries

A useful shortcut.

typedef time_series::TimeSeries<MotorBoardCommand> CommandTimeseries

A useful shortcut.

template<typename Type>
using Ptr = std::shared_ptr<Type>

A useful shortcut.

template<typename Type>
using Vector = std::vector<Type>

A useful shortcut.

typedef time_series::TimeSeries<double> ScalarTimeseries

A useful shortcut.

typedef time_series::Index Index

A useful shortcut.

typedef time_series::TimeSeries<Index> IndexTimeseries

A useful shortcut.

typedef time_series::TimeSeries<MotorBoardStatus> StatusTimeseries

A useful shortcut.

typedef time_series::TimeSeries<MotorBoardCommand> CommandTimeseries

A useful shortcut.

template<typename Type>
using Ptr = std::shared_ptr<Type>

A useful shortcut.

template<typename Type>
using Vector = std::vector<Type>

A useful shortcut.

Public Functions

inline virtual ~MotorBoardInterface()

Destroy the MotorBoardInterface object.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index) const = 0

Getters.

Get the measurements

Parameters:

index – is the kind of measurement we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the last time stamped measurement acquiered.

virtual Ptr<const StatusTimeseries> get_status() const = 0

Get the status of the motor board.

Returns:

Ptr<const StatusTimeseries> is the list of the last status of the card.

virtual Ptr<const ScalarTimeseries> get_control(const int &index) const = 0

input logs

Get the controls to be send.

Parameters:

index – define the kind of control we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the controls to be send.

virtual Ptr<const CommandTimeseries> get_command() const = 0

Get the commands to be send.

Returns:

Ptr<const CommandTimeseries> is the list of the commands to be send.

virtual Ptr<const ScalarTimeseries> get_sent_control(const int &index) const = 0

Get the sent controls.

Parameters:

index – define the kind of control we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the controls sent recently.

virtual Ptr<const CommandTimeseries> get_sent_command() const = 0

Get the sent commands.

Returns:

Ptr<const CommandTimeseries> is the list of the commands sent recently.

virtual void set_control(const double &control, const int &index) = 0

Setters.

set_control save the control internally. In order to actaully send the controls to the network please call “send_if_input_changed”

Parameters:
  • control – is the value of the control.

  • index – define the kind of control we want to send.

virtual void set_command(const MotorBoardCommand &command) = 0

set_command save the command internally.

In order to actaully send the controls to the network please call “send_if_input_changed”

Parameters:

command – is the command to be sent.

virtual void send_if_input_changed() = 0

Actually send the commands and the controls.

inline virtual ~MotorBoardInterface()

Destroy the MotorBoardInterface object.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index) const = 0

Getters.

Get the measurements

Parameters:

index – is the kind of measurement we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the last time stamped measurement acquiered.

virtual Ptr<const StatusTimeseries> get_status() const = 0

Get the status of the motor board.

Returns:

Ptr<const StatusTimeseries> is the list of the last status of the card.

virtual Ptr<const ScalarTimeseries> get_control(const int &index) const = 0

input logs

Get the controls to be send.

Parameters:

index – define the kind of control we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the controls to be send.

virtual Ptr<const CommandTimeseries> get_command() const = 0

Get the commands to be send.

Returns:

Ptr<const CommandTimeseries> is the list of the commands to be send.

virtual Ptr<const ScalarTimeseries> get_sent_control(const int &index) const = 0

Get the sent controls.

Parameters:

index – define the kind of control we are looking for.

Returns:

Ptr<const ScalarTimeseries> is the list of the controls sent recently.

virtual Ptr<const CommandTimeseries> get_sent_command() const = 0

Get the sent commands.

Returns:

Ptr<const CommandTimeseries> is the list of the commands sent recently.

virtual void set_control(const double &control, const int &index) = 0

Setters.

set_control save the control internally. In order to actaully send the controls to the network please call “send_if_input_changed”

Parameters:
  • control – is the value of the control.

  • index – define the kind of control we want to send.

virtual void set_command(const MotorBoardCommand &command) = 0

set_command save the command internally.

In order to actaully send the controls to the network please call “send_if_input_changed”

Parameters:

command – is the command to be sent.

virtual void send_if_input_changed() = 0

Actually send the commands and the controls.

class MotorBoardStatus
#include <motor_board.hpp>

This class represent a 8 bits message that describe the state (enable/disabled) of the card and the two motors.

Public Types

enum ErrorCodes

This is the list of the error codes.

Values:

enumerator NONE

No error.

enumerator ENCODER

Encoder error too high.

enumerator CAN_RECV_TIMEOUT

Timeout for receiving current references exceeded.

enumerator CRIT_TEMP

Motor temperature reached critical value.

Note

This is currently unused as no temperature sensing is done.

enumerator POSCONV

Some error in the SpinTAC Position Convert module.

enumerator POS_ROLLOVER

Position Rollover occured.

enumerator OTHER

Some other error.

enumerator NONE

No error.

enumerator ENCODER

Encoder error too high.

enumerator CAN_RECV_TIMEOUT

Timeout for receiving current references exceeded.

enumerator CRIT_TEMP

Motor temperature reached critical value.

Note

This is currently unused as no temperature sensing is done.

enumerator POSCONV

Some error in the SpinTAC Position Convert module.

enumerator POS_ROLLOVER

Position Rollover occured.

enumerator OTHER

Some other error.

enum ErrorCodes

This is the list of the error codes.

Values:

enumerator NONE

No error.

enumerator ENCODER

Encoder error too high.

enumerator CAN_RECV_TIMEOUT

Timeout for receiving current references exceeded.

enumerator CRIT_TEMP

Motor temperature reached critical value.

Note

This is currently unused as no temperature sensing is done.

enumerator POSCONV

Some error in the SpinTAC Position Convert module.

enumerator POS_ROLLOVER

Position Rollover occured.

enumerator OTHER

Some other error.

enumerator NONE

No error.

enumerator ENCODER

Encoder error too high.

enumerator CAN_RECV_TIMEOUT

Timeout for receiving current references exceeded.

enumerator CRIT_TEMP

Motor temperature reached critical value.

Note

This is currently unused as no temperature sensing is done.

enumerator POSCONV

Some error in the SpinTAC Position Convert module.

enumerator POS_ROLLOVER

Position Rollover occured.

enumerator OTHER

Some other error.

Public Functions

inline void print() const

Simply print the status of the motor board.

inline bool is_ready() const

Check if the all status are green.

inline std::string_view get_error_description() const

Get a human-readable description of the error code.

inline void print() const

Simply print the status of the motor board.

inline bool is_ready() const

Check if the all status are green.

inline std::string_view get_error_description() const

Get a human-readable description of the error code.

Public Members

uint8_t system_enabled

These are the list of bits of the message.

Bits 0 enables/disable of the system (motor board).

uint8_t motor1_enabled

Bits 1 enables/disable of the motor 1.

uint8_t motor1_ready

Bits 2 checks if the motor 1 is ready or not.

uint8_t motor2_enabled

Bits 3 enables/disable of the motor 2.

uint8_t motor2_ready

Bits 4 checks if the motor 2 is ready or not.

uint8_t error_code

This encodes the error codes.

See “ErrorCodes” for more details.

Public Static Functions

static inline constexpr std::string_view get_error_description(uint8_t error_code)
static inline constexpr std::string_view get_error_description(uint8_t error_code)
class MotorInterface : public blmc_drivers::DeviceInterface, public blmc_drivers::DeviceInterface
#include <motor.hpp>

This class declares an interface to the motor.

It allows the user to access the sensors data as well as sending controls. The only control supported for now is the current.

Subclassed by blmc_drivers::Motor, blmc_drivers::Motor

Public Types

enum MeasurementIndex

Here is a list of the different measurement available on the blmc card.

Values:

enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator measurement_count
enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator measurement_count
enum MeasurementIndex

Here is a list of the different measurement available on the blmc card.

Values:

enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator measurement_count
enumerator current
enumerator position
enumerator velocity
enumerator encoder_index
enumerator measurement_count
typedef time_series::TimeSeries<double> ScalarTimeseries

This is a useful alias.

template<typename Type>
using Ptr = std::shared_ptr<Type>

This a useful alias for the shared Pointer creation.

Template Parameters:

Type – is the Class to crate the pointer from.

typedef time_series::TimeSeries<double> ScalarTimeseries

This is a useful alias.

template<typename Type>
using Ptr = std::shared_ptr<Type>

This a useful alias for the shared Pointer creation.

Template Parameters:

Type – is the Class to crate the pointer from.

Public Functions

inline virtual ~MotorInterface()

Destroy the MotorInterface object.

virtual void send_if_input_changed() = 0

Actually send the commands and controls.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index = 0) const = 0

Getters.

Get the measurements.

Parameters:

index

Returns:

Ptr<const ScalarTimeseries> the pointer to the desired measurement history.

virtual Ptr<const ScalarTimeseries> get_current_target() const = 0

Get the current target object.

Returns:

Ptr<const ScalarTimeseries> the list of the current values to be sent.

virtual Ptr<const ScalarTimeseries> get_sent_current_target() const = 0

Get the history of the sent current targets.

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target) = 0

Setters.

Set the current target. This function saves the data internally. Please call send_if_input_changed() to actually send the data.

Parameters:

current_target

virtual void set_command(const MotorBoardCommand &command) = 0

Set the command.

Save internally a command to be apply by the motor board. This function save the command internally. Please call send_if_input_changed() to actually send the data.

Parameters:

command

inline virtual ~MotorInterface()

Destroy the MotorInterface object.

virtual void send_if_input_changed() = 0

Actually send the commands and controls.

virtual Ptr<const ScalarTimeseries> get_measurement(const int &index = 0) const = 0

Getters.

Get the measurements.

Parameters:

index

Returns:

Ptr<const ScalarTimeseries> the pointer to the desired measurement history.

virtual Ptr<const ScalarTimeseries> get_current_target() const = 0

Get the current target object.

Returns:

Ptr<const ScalarTimeseries> the list of the current values to be sent.

virtual Ptr<const ScalarTimeseries> get_sent_current_target() const = 0

Get the history of the sent current targets.

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target) = 0

Setters.

Set the current target. This function saves the data internally. Please call send_if_input_changed() to actually send the data.

Parameters:

current_target

virtual void set_command(const MotorBoardCommand &command) = 0

Set the command.

Save internally a command to be apply by the motor board. This function save the command internally. Please call send_if_input_changed() to actually send the data.

Parameters:

command

class PDController
#include <pd_control.hpp>

This is a basic PD controller to be used in the demos of this package.

Public Functions

inline PDController(std::vector<PairMotorSlider> motor_slider_pairs)

Construct a new PDController object.

Parameters:

motor_slider_pairs

inline ~PDController()

Destroy the PDController object.

inline void start_loop()

This method is a helper to start the thread loop.

Private Functions

void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::vector<PairMotorSlider> motor_slider_pairs_

This is a pair of motor and sliders so that we associate one with the other.

real_time_tools::RealTimeThread rt_thread_

This is the real time thread object.

bool stop_loop

managing the stopping of the loop

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

template<int ORDER>
class Polynome
#include <polynome.hpp>

Simple class that defines \( P(x) \) a polynome of order ORDER.

It provide simple methods to compute \( P(x) \), \( \frac{dP}{dx}(x) \), and \( \frac{dP^2}{dx^2}(x) \)

Template Parameters:

ORDER – is the order of the polynome

Subclassed by blmc_drivers::TimePolynome< ORDER >, blmc_drivers::TimePolynome< ORDER >

Public Functions

Polynome()

Polynome<ORDER> definitions.

Constructor

~Polynome()

Destructor

double compute(double x)

Compute the value.

double compute_derivative(double x)

Compute the value of the derivative.

double compute_sec_derivative(double x)

Compute the value of the second derivative.

void get_coefficients(Coefficients &coefficients) const

Get the coefficients.

void set_coefficients(const Coefficients &coefficients)

Set the coefficients.

inline int degree()
void print() const

Print the coefficient.

Polynome()

Constructor

~Polynome()

Destructor

double compute(double x)

Compute the value.

double compute_derivative(double x)

Compute the value of the derivative.

double compute_sec_derivative(double x)

Compute the value of the second derivative.

void get_coefficients(Coefficients &coefficients) const

Get the coefficients.

void set_coefficients(const Coefficients &coefficients)

Set the coefficients.

inline int degree()
void print() const

Print the coefficient.

Protected Attributes

std::array<double, ORDER + 1> coefficients_

Vector of coefficients.

Private Types

typedef std::array<double, ORDER> Coefficients

Type of the container for the poynome coefficients

typedef std::array<double, ORDER> Coefficients

Type of the container for the poynome coefficients

class SafeMotor : public blmc_drivers::Motor, public blmc_drivers::Motor
#include <motor.hpp>

This class is a safe implementation of the Motor class.

It contains utilities to bound the control input. It could also contains some velocity limits at the motor level and why not some temperature management.

Todo:

the velocity limit should be implemented in a smoother way, and the parameters should be passed in the constructor.

It contains utilities to bound the control input. It could also contains some velocity limits at the motor level and why not some temperature management.

Todo:

the velocity limit should be implemented in a smoother way, and the parameters should be passed in the constructor.

Public Functions

SafeMotor(Ptr<MotorBoardInterface> board, bool motor_id, const double &max_current_target = 2.0, const size_t &history_length = 1000, const double &max_velocity = std::numeric_limits<double>::quiet_NaN())

Construct a new SafeMotor object.

Parameters:
  • board

  • motor_id

  • max_current_target

  • history_length

inline virtual Ptr<const ScalarTimeseries> get_current_target() const

Getters.

Get the _current_target object

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target)

Setters.

Set the current target (Ampere)

Parameters:

current_target

inline void set_max_current(double max_current_target)

Set the max_current_target_ object.

Parameters:

max_current_target

inline void set_max_velocity(double max_velocity)

Set the max_velocity_ constant.

Parameters:

max_velocity

SafeMotor(Ptr<MotorBoardInterface> board, bool motor_id, const double &max_current_target = 2.0, const size_t &history_length = 1000, const double &max_velocity = std::numeric_limits<double>::quiet_NaN())

Construct a new SafeMotor object.

Parameters:
  • board

  • motor_id

  • max_current_target

  • history_length

inline virtual Ptr<const ScalarTimeseries> get_current_target() const

Getters.

Get the _current_target object

Returns:

Ptr<const ScalarTimeseries>

virtual void set_current_target(const double &current_target)

Setters.

Set the current target (Ampere)

Parameters:

current_target

inline void set_max_current(double max_current_target)

Set the max_current_target_ object.

Parameters:

max_current_target

inline void set_max_velocity(double max_velocity)

Set the max_velocity_ constant.

Parameters:

max_velocity

Private Members

double max_current_target_

max_current_target_ is the limit of the current.

double max_velocity_

max_velocity_ limits the motor velocity.

Ptr<ScalarTimeseries> current_target_

History of the target current sent.

class SinePositionControl

This is a basic PD controller to be used in the demos of this package.

Public Functions

inline SinePositionControl(std::vector<SafeMotor_ptr> motor_list)

Construct a new SinePositionControl object.

Parameters:

motor_slider_pairs

inline ~SinePositionControl()

Destroy the SinePositionControl object.

inline void start_loop()

This method is a helper to start the thread loop.

void stop_loop()

Stop the control and dump the data.

inline void set_gains(double kp, double kd)

Private Functions

void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::vector<SafeMotor_ptr> motor_list_

This is list of motors.

real_time_tools::RealTimeThread rt_thread_

This is the real time thread object.

bool stop_loop_

managing the stopping of the loop

unsigned memory_buffer_size_

memory_buffer_size_ is the max size of the memory buffer.

std::vector<std::deque<double>> encoders_

Encoder data.

std::vector<std::deque<double>> velocities_

Velocity data.

std::vector<std::deque<double>> currents_

current data

std::vector<std::deque<double>> control_buffer_

control_buffer_

double kp_

Controller proportional gain.

double kd_

Controller derivative gain.

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

class SineTorqueControl

This is a basic PD controller to be used in the demos of this package.

Public Functions

inline SineTorqueControl(std::vector<SafeMotor_ptr> motor_list)

Construct a new SineTorqueControl object.

Parameters:

motor_slider_pairs

inline ~SineTorqueControl()

Destroy the SineTorqueControl object.

inline void start_loop()

This method is a helper to start the thread loop.

void stop_loop()

Stop the control and dump the data.

Private Functions

void loop()

this is a simple control loop which runs at a kilohertz.

it reads the measurement from the analog sensor, in this case the slider. then it scales it and sends it as the current target to the motor.

Private Members

std::vector<SafeMotor_ptr> motor_list_

This is list of motors.

real_time_tools::RealTimeThread rt_thread_

This is the real time thread object.

bool stop_loop_

managing the stopping of the loop

unsigned memory_buffer_size_

memory_buffer_size_ is the max size of the memory buffer.

std::vector<std::deque<double>> encoders_

Encoder data.

std::vector<std::deque<double>> velocities_

Velocity data.

std::vector<std::deque<double>> currents_

current data

std::vector<std::deque<double>> control_buffer_

control_buffer_

Private Static Functions

static inline THREAD_FUNCTION_RETURN_TYPE loop(void *instance_pointer)

this function is just a wrapper around the actual loop function, such that it can be spawned as a posix thread.

template<int ORDER>
class TimePolynome : public blmc_drivers::Polynome<ORDER>, public blmc_drivers::Polynome<ORDER>
#include <polynome.hpp>

Simple class that defines \( P(t) \) a polynome of order ORDER.

With \( t \) being the time in any units. It provide simple methods to compute safely \( P(time) \), \( \frac{dP}{dt}(t) \), and \( \frac{dP^2}{dt^2}(t) \)

Template Parameters:

ORDER

Public Functions

inline TimePolynome()

Constructor

inline ~TimePolynome()

Destructor

double compute(double t)

TimePolynome<ORDER> definitions.

Compute the value.

double compute_derivative(double t)

Compute the value of the derivative.

double compute_sec_derivative(double t)

Compute the value of the second derivative.

inline double get_final_time() const
inline double get_init_pose() const
inline double get_init_speed() const
inline double get_init_acc() const
inline double get_final_pose() const
inline double get_final_speed() const
inline double get_final_acc() const
void set_parameters(double final_time, double init_pose, double init_speed, double final_pose)

Computes a polynome trajectory according to the following constraints:

\[\begin{split}\begin{eqnarray*} P(0) &=& init_pose \\ P(0) &=& init_speed = 0 \\ P(0) &=& init_acc = 0 \\ P(final_time_) &=& final_pose \\ P(final_time_) &=& final_speed = 0 \\ P(final_time_) &=& final_acc = 0 \end{eqnarray*}\end{split}\]

Parameters:
  • final_time – is used in the constraints.

  • init_pose – is used in the constraints.

  • init_speed – is used in the constraints.

  • final_pose – is used in the constraints.

inline TimePolynome()

Constructor

inline ~TimePolynome()

Destructor

double compute(double t)

Compute the value.

double compute_derivative(double t)

Compute the value of the derivative.

double compute_sec_derivative(double t)

Compute the value of the second derivative.

inline double get_final_time() const
inline double get_init_pose() const
inline double get_init_speed() const
inline double get_init_acc() const
inline double get_final_pose() const
inline double get_final_speed() const
inline double get_final_acc() const
void set_parameters(double final_time, double init_pose, double init_speed, double final_pose)

Computes a polynome trajectory according to the following constraints:

\[\begin{split}\begin{eqnarray*} P(0) &=& init_pose \\ P(0) &=& init_speed = 0 \\ P(0) &=& init_acc = 0 \\ P(final_time_) &=& final_pose \\ P(final_time_) &=& final_speed = 0 \\ P(final_time_) &=& final_acc = 0 \end{eqnarray*}\end{split}\]

Parameters:
  • final_time – is used in the constraints.

  • init_pose – is used in the constraints.

  • init_speed – is used in the constraints.

  • final_pose – is used in the constraints.

Protected Attributes

double final_time_

store the inputs for later access

double init_pose_

store the inputs for later access

double init_speed_

store the inputs for later access

double init_acc_

store the inputs for later access

double final_pose_

store the inputs for later access

double final_speed_

store the inputs for later access

double final_acc_

store the inputs for later access

namespace blmc_drivers

This namespace is the standard namespace of the package.

Typedefs

typedef std::shared_ptr<blmc_drivers::SafeMotor> SafeMotor_ptr

This is a simple shortcut.

typedef std::shared_ptr<blmc_drivers::AnalogSensor> Slider_ptr

This is a simple shortcut.

typedef std::pair<SafeMotor_ptr, Slider_ptr> PairMotorSlider

This is a simple shortcut.

typedef blmc_drivers::MotorInterface::MeasurementIndex mi
typedef std::shared_ptr<BlmcJointModule> BlmcJointModule_ptr

BlmcJointModule_ptr shortcut for the shared pointer BlmcJointModule type.

Enums

enum class HomingReturnCode

Possible return values of the homing.

Values:

enumerator NOT_INITIALIZED

Homing was not initialized and can therefore not be performed.

enumerator RUNNING

Homing is currently running.

enumerator SUCCEEDED

Homing is succeeded.

enumerator FAILED

Homing failed.

enumerator NOT_INITIALIZED

Homing was not initialized and can therefore not be performed.

enumerator RUNNING

Homing is currently running.

enumerator SUCCEEDED

Homing is succeeded.

enumerator FAILED

Homing failed.

enum class GoToReturnCode

Possible return values of the go_to.

Values:

enumerator RUNNING

GoTo is currently running.

enumerator SUCCEEDED

Position has been reached succeeded.

enumerator FAILED

Robot is stuck(hit an obstacle) before reaching its final position.

enumerator RUNNING

GoTo is currently running.

enumerator SUCCEEDED

Position has been reached succeeded.

enumerator FAILED

Robot is stuck(hit an obstacle) before reaching its final position.

enum class HomingReturnCode

Possible return values of the homing.

Values:

enumerator NOT_INITIALIZED

Homing was not initialized and can therefore not be performed.

enumerator RUNNING

Homing is currently running.

enumerator SUCCEEDED

Homing is succeeded.

enumerator FAILED

Homing failed.

enumerator NOT_INITIALIZED

Homing was not initialized and can therefore not be performed.

enumerator RUNNING

Homing is currently running.

enumerator SUCCEEDED

Homing is succeeded.

enumerator FAILED

Homing failed.

enum class GoToReturnCode

Possible return values of the go_to.

Values:

enumerator RUNNING

GoTo is currently running.

enumerator SUCCEEDED

Position has been reached succeeded.

enumerator FAILED

Robot is stuck(hit an obstacle) before reaching its final position.

enumerator RUNNING

GoTo is currently running.

enumerator SUCCEEDED

Position has been reached succeeded.

enumerator FAILED

Robot is stuck(hit an obstacle) before reaching its final position.

Functions

template<typename Type>
std::vector<std::shared_ptr<Type>> create_vector_of_pointers(const size_t &size, const size_t &length)

Create a vector of pointers.

Template Parameters:

Type – of the data

Parameters:
  • size – is number of pointers to be created.

  • length – is the dimension of the data arrays.

Returns:

Vector<Ptr<Type>> which is the a list of list of data of type Type

namespace osi

Common include.

osi stands for Operating System Interface.

Todo:

This workspace should be replaced eventually by the real_time_tools package.

osi stands for Operating System Interface.

Todo:

This workspace should be replaced eventually by the real_time_tools package.

Typedefs

typedef std::mutex Mutex

Wrapper around the posix specific Mutex implementation.

typedef std::condition_variable ConditionVariable

Wrapper around the posix specific ConditionVariable implementation.

Functions

inline void send_to_can_device(int fd, const void *buf, size_t len, int flags, const struct sockaddr *to, socklen_t tolen)

Use the osi workspace API to communicate with the can bus.

/todo Manuel can you describe the argument of this function?

Parameters:
  • fd

  • buf

  • len

  • flags

  • to

  • tolen

inline void close_can_device(int socket)

This function is closing a socket on the Can device.

It is os independent.

Parameters:

socket

inline void receive_message_from_can_device(int fd, struct msghdr *msg, int flags)

Poll? a message from the CAN device.

Todo:

Manuel can you confrim this? And precise the arguments of the function?

Todo:

Manuel can you confrim this? And precise the arguments of the function?

Parameters:
  • fd

  • msg

  • flags

  • fd

  • msg

  • flags

inline void initialize_realtime_printing()

This function is needed in xenomai to initialize the real time console display of text.

inline void sleep_ms(const double &sleep_time_ms)

This function uses eather the xenomai API or the posix one.

Parameters:

sleep_time_ms – is the sleeping time in milli seconds.

inline double get_current_time_ms()

Get the current time in millisecond.

Todo:

remove as the one form the Timer class is much better embeded.

Todo:

remove as the one form the Timer class is much better embeded.

Returns:

double which is the time in milli second

Returns:

double which is the time in milli second

inline void make_this_thread_realtime()

This methd is requiered in xenomai to create a real time thread.

file const_torque_control.cpp
#include “const_torque_control.hpp
#include <fstream>
#include “real_time_tools/spinner.hpp”
#include “real_time_tools/timer.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file const_torque_control.hpp
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/motor.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file demo_1_motor.cpp
#include <tuple>
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/can_bus.hpp”
#include “blmc_drivers/devices/motor.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Typedefs

typedef std::tuple<std::shared_ptr<blmc_drivers::MotorInterface>, std::shared_ptr<blmc_drivers::AnalogSensorInterface>> MotorAndSlider

Functions

static THREAD_FUNCTION_RETURN_TYPE control_loop(void *hardware_ptr)
static THREAD_FUNCTION_RETURN_TYPE printing_loop(void *hardware_ptr)
int main(int argc, char **argv)
file demo_1_motor_print_everything.cpp
#include <tuple>
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/can_bus.hpp”
#include “blmc_drivers/devices/motor.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Typedefs

typedef std::tuple<std::shared_ptr<blmc_drivers::MotorInterface>, std::shared_ptr<blmc_drivers::AnalogSensorInterface>> MotorAndSlider

Functions

static THREAD_FUNCTION_RETURN_TYPE control_loop(void *hardware_ptr)
static THREAD_FUNCTION_RETURN_TYPE printing_loop(void *hardware_ptr)
int main(int, char**)
file demo_2_motors.cpp
#include <signal.h>
#include <atomic>
#include <pd_control.hpp>

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_3_motors.cpp
#include <signal.h>
#include <atomic>
#include “pd_control.hpp

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int s)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int argc, char **argv)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_8_motors.cpp
#include <signal.h>
#include <atomic>
#include <pd_control.hpp>

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_const_torque_1_motor.cpp
#include <signal.h>
#include <atomic>
#include “const_torque_control.hpp

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_leg.cpp
#include <math.h>
#include <blmc_drivers/devices/analog_sensor.hpp>
#include <blmc_drivers/devices/leg.hpp>
#include <blmc_drivers/devices/motor.hpp>
#include “real_time_tools/spinner.hpp”
#include “real_time_tools/timer.hpp”
#include <signal.h>
#include <atomic>
#include <pd_control.hpp>

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)
file demo_print_analog_sensors.cpp
#include <iomanip>
#include <real_time_tools/spinner.hpp>
#include <blmc_drivers/devices/analog_sensor.hpp>
#include <blmc_drivers/devices/can_bus.hpp>
#include <blmc_drivers/devices/motor_board.hpp>

Print ADC measurements in a loop.

Copyright

2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

int main(int argc, char **argv)
file demo_sine_position_1_motor.cpp
#include <signal.h>
#include <atomic>

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_sine_torque_1_motor.cpp
#include <signal.h>
#include <atomic>
#include “sine_torque_control.hpp

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file demo_single_board.cpp
#include <signal.h>
#include <atomic>
#include “pd_control.hpp

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

Functions

std::atomic_bool StopDemos (false)

This boolean is here to kill cleanly the application upon ctrl+c.

void my_handler(int)

This function is the callback upon a ctrl+c call from the terminal.

Parameters:

s

int main(int, char**)

This is the main demo program.

Parameters:
  • argc

  • argv

Returns:

int

file pd_control.cpp
#include “pd_control.hpp
#include “real_time_tools/spinner.hpp”
#include “real_time_tools/timer.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file pd_control.hpp
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/motor.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file sine_position_control.cpp
#include <fstream>
#include “real_time_tools/spinner.hpp”
#include “real_time_tools/timer.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file sine_position_control.hpp
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/motor.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file sine_torque_control.cpp
#include “sine_torque_control.hpp
#include <fstream>
#include “real_time_tools/spinner.hpp”
#include “real_time_tools/timer.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file sine_torque_control.hpp
#include “blmc_drivers/devices/analog_sensor.hpp”
#include “blmc_drivers/devices/motor.hpp”

Copyright

Copyright (c) 2018-2020, New York University and Max Planck Gesellschaft, License BSD-3-Clause

file blmc_joint_module.hpp
#include <math.h>
#include <array>
#include <iostream>
#include <stdexcept>
#include <Eigen/Eigen>
#include “blmc_drivers/devices/motor.hpp”
#include “blmc_drivers/utils/polynome.hpp”

License:

License BSD-3-Clause

Author

Manuel Wuthrich

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file blmc_joint_module.hpp
#include <math.h>
#include <array>
#include <iostream>
#include <stdexcept>
#include <Eigen/Eigen>
#include “blmc_drivers/devices/motor.hpp”
#include “blmc_drivers/utils/polynome.hpp”

License:

License BSD-3-Clause

Author

Manuel Wuthrich

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file analog_sensor.hpp
#include <memory>
#include <string>
#include <time_series/time_series.hpp>
#include “real_time_tools/timer.hpp”
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file analog_sensor.hpp
#include <memory>
#include <string>
#include <time_series/time_series.hpp>
#include “real_time_tools/timer.hpp”
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file can_bus.hpp
#include <memory>
#include <string>
#include <real_time_tools/iostream.hpp>
#include <real_time_tools/spinner.hpp>
#include <real_time_tools/thread.hpp>
#include <real_time_tools/threadsafe/threadsafe_object.hpp>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/utils/os_interface.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file can_bus.hpp
#include <memory>
#include <string>
#include <real_time_tools/iostream.hpp>
#include <real_time_tools/spinner.hpp>
#include <real_time_tools/thread.hpp>
#include <real_time_tools/threadsafe/threadsafe_object.hpp>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/utils/os_interface.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file device_interface.hpp

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file device_interface.hpp

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file leg.hpp
#include <map>
#include <memory>
#include <string>
#include <time_series/time_series.hpp>
#include <blmc_drivers/devices/device_interface.hpp>
#include <blmc_drivers/devices/motor.hpp>

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file leg.hpp
#include <map>
#include <memory>
#include <string>
#include <time_series/time_series.hpp>
#include <blmc_drivers/devices/device_interface.hpp>
#include <blmc_drivers/devices/motor.hpp>

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file motor.hpp
#include <memory>
#include <string>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019-2020, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file motor.hpp
#include <memory>
#include <string>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/devices/motor_board.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019-2020, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file motor_board.hpp
#include <memory>
#include <string_view>
#include <real_time_tools/thread.hpp>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/can_bus.hpp”
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/utils/os_interface.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file motor_board.hpp
#include <memory>
#include <string_view>
#include <real_time_tools/thread.hpp>
#include <real_time_tools/timer.hpp>
#include <time_series/time_series.hpp>
#include “blmc_drivers/devices/can_bus.hpp”
#include “blmc_drivers/devices/device_interface.hpp”
#include “blmc_drivers/utils/os_interface.hpp”

License:

License BSD-3-Clause

Copyright

Copyright (c) 2019, New York University and Max Planck Gesellschaft.

Date

2019-07-11

file os_interface.hpp
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <limits.h>
#include <real_time_tools/timer.hpp>
#include <iostream>
#include <condition_variable>
#include <mutex>
#include <sstream>
#include <sys/mman.h>

Author

Manuel Wuthrich (manuel.wuthrich@gmail.com)

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Version

0.1

Date

2018-11-27

Copyright

Copyright (c) 2018

Defines

rt_fprintf

Create a common type_def to wrap xenomai and posix.

rt_printf

Create a common type_def to wrap xenomai and posix.

rt_dev_socket

Create a common type_def to wrap xenomai and posix.

rt_dev_ioctl

Create a common type_def to wrap xenomai and posix.

rt_dev_close

Create a common type_def to wrap xenomai and posix.

rt_dev_setsockopt

Create a common type_def to wrap xenomai and posix.

rt_dev_bind

Create a common type_def to wrap xenomai and posix.

rt_dev_recvmsg

Create a common type_def to wrap xenomai and posix.

rt_dev_sendto

Create a common type_def to wrap xenomai and posix.

Typedefs

typedef struct can_frame can_frame_t

xeno specific include

Define typedefs to make code compatible with Xenomai code.

Create a common type_def to wrap xenomai and posix.

typedef canid_t can_id_t

Create a common type_def to wrap xenomai and posix.

typedef uint64_t nanosecs_abs_t

Create a common type_def to wrap xenomai and posix.

file os_interface.hpp
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <limits.h>
#include <real_time_tools/timer.hpp>
#include <iostream>
#include <condition_variable>
#include <mutex>
#include <sstream>
#include <sys/mman.h>

Author

Manuel Wuthrich (manuel.wuthrich@gmail.com)

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Version

0.1

Date

2018-11-27

Copyright

Copyright (c) 2018

Defines

rt_fprintf

Create a common type_def to wrap xenomai and posix.

rt_printf

Create a common type_def to wrap xenomai and posix.

rt_dev_socket

Create a common type_def to wrap xenomai and posix.

rt_dev_ioctl

Create a common type_def to wrap xenomai and posix.

rt_dev_close

Create a common type_def to wrap xenomai and posix.

rt_dev_setsockopt

Create a common type_def to wrap xenomai and posix.

rt_dev_bind

Create a common type_def to wrap xenomai and posix.

rt_dev_recvmsg

Create a common type_def to wrap xenomai and posix.

rt_dev_sendto

Create a common type_def to wrap xenomai and posix.

Typedefs

typedef struct can_frame can_frame_t

xeno specific include

Define typedefs to make code compatible with Xenomai code.

Create a common type_def to wrap xenomai and posix.

typedef canid_t can_id_t

Create a common type_def to wrap xenomai and posix.

typedef uint64_t nanosecs_abs_t

Create a common type_def to wrap xenomai and posix.

file polynome.hpp
#include <array>
#include “blmc_drivers/utils/polynome.hxx”

Polynomes object for trajectories.

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Version

0.1

Date

2019-11-06

Copyright

Copyright (c) 2019

file polynome.hpp
#include <array>
#include “blmc_drivers/utils/polynome.hxx”

Polynomes object for trajectories.

Author

Maximilien Naveau (maximilien.naveau@gmail.com)

Version

0.1

Date

2019-11-06

Copyright

Copyright (c) 2019

file polynome.hxx
#include <iostream>
#include <vector>
file polynome.hxx
#include <iostream>
#include <vector>
page license

File analog_sensor.hpp

License BSD-3-Clause

File analog_sensor.hpp

License BSD-3-Clause

File blmc_joint_module.hpp

License BSD-3-Clause

File blmc_joint_module.hpp

License BSD-3-Clause

File can_bus.hpp

License BSD-3-Clause

File can_bus.hpp

License BSD-3-Clause

File device_interface.hpp

License BSD-3-Clause

File device_interface.hpp

License BSD-3-Clause

File leg.hpp

License BSD-3-Clause

File leg.hpp

License BSD-3-Clause

File motor.hpp

License BSD-3-Clause

File motor.hpp

License BSD-3-Clause

File motor_board.hpp

License BSD-3-Clause

File motor_board.hpp

License BSD-3-Clause

page deprecated

Member blmc_drivers::BlmcJointModule::calibrate  (double &angle_zero_to_index, double &index_angle, bool mechanical_calibration=false)

!!!!!!!

!!!!!!!

page todo

Member blmc_drivers::CanBusMotorBoard::pause_motors  ()

: this function should go away, and we should add somewhere a warning in case there is a timeout

: this function should go away, and we should add somewhere a warning in case there is a timeout

Class blmc_drivers::SafeMotor

the velocity limit should be implemented in a smoother way, and the parameters should be passed in the constructor.

the velocity limit should be implemented in a smoother way, and the parameters should be passed in the constructor.

Namespace osi

This workspace should be replaced eventually by the real_time_tools package.

This workspace should be replaced eventually by the real_time_tools package.

Member osi::get_current_time_ms  ()

remove as the one form the Timer class is much better embeded.

remove as the one form the Timer class is much better embeded.

Member osi::receive_message_from_can_device  (int fd, struct msghdr *msg, int flags)

Manuel can you confrim this? And precise the arguments of the function?

Manuel can you confrim this? And precise the arguments of the function?

dir include/blmc_drivers
dir install/blmc_drivers
dir install/blmc_drivers/include/blmc_drivers
dir demos
dir include/blmc_drivers/devices
dir install/blmc_drivers/include/blmc_drivers/devices
dir include
dir install/blmc_drivers/include
dir install
dir include/blmc_drivers/utils
dir install/blmc_drivers/include/blmc_drivers/utils