TI_BoardControl Class Reference

#include <ti_boardcontrol.h>

Collaboration diagram for TI_BoardControl:

Public Member Functions
	TI_BoardControl ()=default
void	init (float side_sign)
void	run_ti_board_iteration ()
void	reset_ti_board_data ()
void	reset_ti_board_command ()
void	set_link_lengths (float l1, float l2, float l3)

Public Attributes
TiBoardCommand	command
TiBoardData	data_structure
TiBoardData *	data

Private Member Functions
void	kinematics (const float side_sign, const float q[3], const float dq[3], float *p, float *v, float J[][3])
void	impedanceControl (const float side_sign, const float q[3], const float dq[3], const float position_des[3], const float velocity_des[3], const float kp[3], const float kd[3], const float force_bias[3], const float torque_bias[3], float *position, float *velocity, float *force, float *torque)

Private Attributes
float	_side_sign
float	_l1
float	_l2
float	_l3
bool	link_lengths_set = false

Detailed Description

Definition at line 33 of file ti_boardcontrol.h.

Constructor & Destructor Documentation

TI_BoardControl::TI_BoardControl ( )

default

Member Function Documentation

void TI_BoardControl::impedanceControl ( const float  side_sign, const float  q[3], const float  dq[3], const float  position_des[3], const float  velocity_des[3], const float  kp[3], const float  kd[3], const float  force_bias[3], const float  torque_bias[3], float *  position, float *  velocity, float *  force, float *  torque  )

private

Definition at line 71 of file ti_boardcontrol.cpp.

References kinematics().

                                                                   {
  float Jacobian[3][3];
  kinematics(side_sign, q, dq, position, velocity, Jacobian);

  force[0] = kp[0] * (position_des[0] - position[0]) +
             kd[0] * (velocity_des[0] - velocity[0]) + force_bias[0];
  force[1] = kp[1] * (position_des[1] - position[1]) +
             kd[1] * (velocity_des[1] - velocity[1]) + force_bias[1];
  force[2] = kp[2] * (position_des[2] - position[2]) +
             kd[2] * (velocity_des[2] - velocity[2]) + force_bias[2];

  torque[0] = (force[0] * Jacobian[0][0] + force[1] * Jacobian[1][0] +
               force[2] * Jacobian[2][0]) +
              torque_bias[0];
  torque[1] = (force[0] * Jacobian[0][1] + force[1] * Jacobian[1][1] +
               force[2] * Jacobian[2][1]) +
              torque_bias[1];
  torque[2] = (force[0] * Jacobian[0][2] + force[1] * Jacobian[1][2] +
               force[2] * Jacobian[2][2]) +
              torque_bias[2];
}

Here is the call graph for this function:

Here is the caller graph for this function:

void TI_BoardControl::init

(

float

side_sign

)

Definition at line 7 of file ti_boardcontrol.cpp.

References _side_sign, data, and data_structure.

                                          {
  std::cout << "[Cheetah Control] Hello! I am the TI board with side sign: "
            << side_sign << "\n";
  this->_side_sign = side_sign;
  data = &data_structure;
}

Here is the caller graph for this function:

void TI_BoardControl::kinematics ( const float  side_sign, const float  q[3], const float  dq[3], float *  p, float *  v, float  J[][3]  )

private

Definition at line 97 of file ti_boardcontrol.cpp.

References _l1, _l2, and _l3.

                                               {
  //    ti_flt l1 = 0.045;
  //    ti_flt l2 = .342;
  //    ti_flt l3 = .345;

  //    if(get_cheetah() == 4)
  //    {
  //        l1 = .062;
  //        l2 = .209;
  //        l3 = .175;
  //    }

  const float s1 = sin(q[0]), s2 = sin(q[1]), s3 = sin(q[2]);
  const float c1 = cos(q[0]), c2 = cos(q[1]), c3 = cos(q[2]);
  const float c23 = c2 * c3 - s2 * s3;
  const float s23 = s2 * c3 + c2 * s3;

  p[0] = _l3 * s23 + _l2 * s2;
  p[1] = _l1 * side_sign * c1 + _l3 * (s1 * c23) + _l2 * c2 * s1;
  p[2] = _l1 * side_sign * s1 - _l3 * (c1 * c23) - _l2 * c1 * c2;

  J[0][0] = 0;
  J[0][1] = _l3 * c23 + _l2 * c2;
  J[0][2] = _l3 * c23;
  J[1][0] = _l3 * c1 * c23 + _l2 * c1 * c2 - _l1 * side_sign * s1;
  J[1][1] = -_l3 * s1 * s23 - _l2 * s1 * s2;
  J[1][2] = -_l3 * s1 * s23;
  J[2][0] = _l3 * s1 * c23 + _l2 * c2 * s1 + _l1 * side_sign * c1;
  J[2][1] = _l3 * c1 * s23 + _l2 * c1 * s2;
  J[2][2] = _l3 * c1 * s23;

  v[0] = 0;
  v[1] = 0;
  v[2] = 0;
  int i, j;
  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      v[i] += J[i][j] * dq[j];
    }
  }
}

Here is the caller graph for this function:

void TI_BoardControl::reset_ti_board_command

(

)

Definition at line 28 of file ti_boardcontrol.cpp.

References command, data_structure, TiBoardCommand::kd, TiBoardCommand::kp, TiBoardCommand::position_des, TiBoardData::tau_des, and TiBoardCommand::velocity_des.

                                             {
  command.position_des[0] = 0;
  command.position_des[1] = 0;
  command.position_des[2] = 0;

  std::fill_n(command.velocity_des, 3, 0);
  std::fill_n(command.kp, 3, 0);
  std::fill_n(command.kd, 3, 0);
  std::fill_n(data_structure.tau_des, 3, 0);
}

Here is the caller graph for this function:

void TI_BoardControl::reset_ti_board_data

(

)

Definition at line 21 of file ti_boardcontrol.cpp.

References data_structure, TiBoardData::ethercat_count_ti, TiBoardData::loop_count_ti, TiBoardData::microtime_ti, and TiBoardData::tau_des.

                                          {
  std::fill_n(data_structure.tau_des, 3, 0);
  data_structure.loop_count_ti = 0;
  data_structure.ethercat_count_ti = 0;
  data_structure.microtime_ti = 0;
}

Here is the caller graph for this function:

void TI_BoardControl::run_ti_board_iteration

(

)

Definition at line 39 of file ti_boardcontrol.cpp.

References _side_sign, command, data, TiBoardData::dq, TiBoardCommand::enable, TiBoardData::force, TiBoardCommand::force_ff, impedanceControl(), TiBoardCommand::kd, kinematics(), TiBoardCommand::kp, link_lengths_set, TiBoardData::loop_count_ti, TiBoardCommand::max_torque, TiBoardData::position, TiBoardCommand::position_des, TiBoardData::q, TiBoardData::tau_des, TiBoardCommand::tau_ff, TiBoardData::velocity, and TiBoardCommand::velocity_des.

                                             {
  if (!link_lengths_set) {
    std::cout << "[TI Board] Error.  Link lengths haven't been set.\n";
    return;
  }

  if (command.enable == 1 && command.max_torque > 1e-9) {
    impedanceControl(_side_sign, data->q, data->dq, command.position_des,
                     command.velocity_des, command.kp, command.kd,
                     command.force_ff, command.tau_ff, data->position,
                     data->velocity, data->force, data->tau_des);

    for (int i = 0; i < 3; ++i) {
      if (std::abs(data->tau_des[i]) > command.max_torque) {
        data->tau_des[i] =
            data->tau_des[i] / std::abs(data->tau_des[i]) * command.max_torque;
      }
    }
  } else {
    float Jacobian[3][3];
    kinematics(_side_sign, data->q, data->dq, data->position, data->velocity,
               Jacobian);
    data->force[0] = 0;
    data->force[1] = 0;
    data->force[2] = 0;
    data->tau_des[0] = 0;
    data->tau_des[1] = 0;
    data->tau_des[2] = 0;
  }
  data->loop_count_ti += 1;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void TI_BoardControl::set_link_lengths	(	float	l1,
		float	l2,
		float	l3
	)

Definition at line 14 of file ti_boardcontrol.cpp.

References _l1, _l2, _l3, and link_lengths_set.

                                                                   {
  link_lengths_set = true;
  this->_l1 = l1;
  this->_l2 = l2;
  this->_l3 = l3;
}

Here is the caller graph for this function:

Member Data Documentation

float TI_BoardControl::_l1

private

Definition at line 58 of file ti_boardcontrol.h.

float TI_BoardControl::_l2

private

Definition at line 58 of file ti_boardcontrol.h.

float TI_BoardControl::_l3

private

Definition at line 58 of file ti_boardcontrol.h.

float TI_BoardControl::_side_sign

private

Definition at line 57 of file ti_boardcontrol.h.

TiBoardCommand TI_BoardControl::command

Definition at line 41 of file ti_boardcontrol.h.

TiBoardData* TI_BoardControl::data

Definition at line 45 of file ti_boardcontrol.h.

TiBoardData TI_BoardControl::data_structure

Definition at line 42 of file ti_boardcontrol.h.

bool TI_BoardControl::link_lengths_set = false

private

Definition at line 59 of file ti_boardcontrol.h.

---

The documentation for this class was generated from the following files:

• ti_boardcontrol.h
• ti_boardcontrol.cpp