LegController< T > Class Template Reference

#include <LegController.h>

Inheritance diagram for LegController< T >:

Collaboration diagram for LegController< T >:

Public Member Functions
	LegController (Quadruped< T > &quad)
void	zeroCommand ()
void	edampCommand (RobotType robot, T gain)
void	updateData (const SpiData *spiData)
void	updateData (const TiBoardData *tiBoardData)
void	updateCommand (SpiCommand *spiCommand)
void	updateCommand (TiBoardCommand *tiBoardCommand)
void	setEnabled (bool enabled)
void	setLcm (leg_control_data_lcmt *data, leg_control_command_lcmt *command)
void	setMaxTorqueCheetah3 (T tau)

Public Attributes
LegControllerCommand< T >	commands [4]
LegControllerData< T >	datas [4]
Quadruped< T > &	_quadruped
bool	_legsEnabled = false
T	_maxTorque = 0

Detailed Description

template<typename T>
class LegController< T >

Definition at line 49 of file LegController.h.

Constructor & Destructor Documentation

template<typename T>

LegController< T >::LegController ( Quadruped< T > &  quad )

inline

Definition at line 51 of file LegController.h.

                                    : _quadruped(quad) {
    for (auto& data : datas) data.setQuadruped(_quadruped);
  }

Member Function Documentation

template<typename T>

void LegController< T >::edampCommand	(	RobotType	robot,
		T	gain
	)

Set the leg to edamp. This overwrites all command data and generates an emergency damp command using the given gain. For the mini-cheetah, the edamp gain is Nm/(rad/s), and for the Cheetah 3 it is N/m. You still must call updateCommand for this command to end up in the low-level command data!

Definition at line 62 of file LegController.cpp.

References CHEETAH_3.

                                                           {
  zeroCommand();
  if (robot == RobotType::CHEETAH_3) {
    for (int leg = 0; leg < 4; leg++) {
      for (int axis = 0; axis < 3; axis++) {
        commands[leg].kdCartesian(axis, axis) = gain;
      }
    }
  } else {  // mini-cheetah
    for (int leg = 0; leg < 4; leg++) {
      for (int axis = 0; axis < 3; axis++) {
        commands[leg].kdJoint(axis, axis) = gain;
      }
    }
  }
}

template<typename T>

void LegController< T >::setEnabled ( bool  enabled )

inline

Definition at line 61 of file LegController.h.

{ _legsEnabled = enabled; };

Here is the caller graph for this function:

template<typename T >

void LegController< T >::setLcm	(	leg_control_data_lcmt *	data,
		leg_control_command_lcmt *	command
	)

Definition at line 202 of file LegController.cpp.

                                                                                                  {
    for(int leg = 0; leg < 4; leg++) {
        for(int axis = 0; axis < 3; axis++) {
            int idx = leg*3 + axis;
            lcmData->q[idx] = datas[leg].q[axis];
            lcmData->qd[idx] = datas[leg].qd[axis];
            lcmData->p[idx] = datas[leg].p[axis];
            lcmData->v[idx] = datas[leg].v[axis];
            lcmData->tau_est[idx] = datas[leg].tauEstimate[axis];

            lcmCommand->tau_ff[idx] = commands[leg].tauFeedForward[axis];
            lcmCommand->f_ff[idx] = commands[leg].forceFeedForward[axis];
            lcmCommand->q_des[idx] = commands[leg].qDes[axis];
            lcmCommand->qd_des[idx] = commands[leg].qdDes[axis];
            lcmCommand->p_des[idx] = commands[leg].pDes[axis];
            lcmCommand->v_des[idx] = commands[leg].vDes[axis];
            lcmCommand->kp_cartesian[idx] = commands[leg].kpCartesian(axis, axis);
            lcmCommand->kd_cartesian[idx] = commands[leg].kdCartesian(axis, axis);
            lcmCommand->kp_joint[idx] = commands[leg].kpJoint(axis, axis);
            lcmCommand->kd_joint[idx] = commands[leg].kdJoint(axis, axis);
        }
    }
}

Here is the caller graph for this function:

template<typename T>

void LegController< T >::setMaxTorqueCheetah3 ( T  tau )

inline

Set the maximum torque. This only works on cheetah 3!

Definition at line 67 of file LegController.h.

{ _maxTorque = tau; }

Here is the caller graph for this function:

template<typename T >

void LegController< T >::updateCommand

(

SpiCommand *

spiCommand

)

Update the "leg command" for the SPIne board message

Definition at line 126 of file LegController.cpp.

References SpiCommand::flags, SpiCommand::kd_abad, SpiCommand::kd_hip, SpiCommand::kd_knee, SpiCommand::kp_abad, SpiCommand::kp_hip, SpiCommand::kp_knee, SpiCommand::q_des_abad, SpiCommand::q_des_hip, SpiCommand::q_des_knee, SpiCommand::qd_des_abad, SpiCommand::qd_des_hip, SpiCommand::qd_des_knee, SpiCommand::tau_abad_ff, SpiCommand::tau_hip_ff, and SpiCommand::tau_knee_ff.

                                                           {
  for (int leg = 0; leg < 4; leg++) {
    // tauFF
    Vec3<T> legTorque = commands[leg].tauFeedForward;

    // forceFF
    Vec3<T> footForce = commands[leg].forceFeedForward;

    // cartesian PD
    footForce +=
        commands[leg].kpCartesian * (commands[leg].pDes - datas[leg].p);
    footForce +=
        commands[leg].kdCartesian * (commands[leg].vDes - datas[leg].v);

    // Torque
    legTorque += datas[leg].J.transpose() * footForce;

    // set command:
    spiCommand->tau_abad_ff[leg] = legTorque(0);
    spiCommand->tau_hip_ff[leg] = legTorque(1);
    spiCommand->tau_knee_ff[leg] = legTorque(2);

    // joint space pd
    // joint space PD
    spiCommand->kd_abad[leg] = commands[leg].kdJoint(0, 0);
    spiCommand->kd_hip[leg] = commands[leg].kdJoint(1, 1);
    spiCommand->kd_knee[leg] = commands[leg].kdJoint(2, 2);

    spiCommand->kp_abad[leg] = commands[leg].kpJoint(0, 0);
    spiCommand->kp_hip[leg] = commands[leg].kpJoint(1, 1);
    spiCommand->kp_knee[leg] = commands[leg].kpJoint(2, 2);

    spiCommand->q_des_abad[leg] = commands[leg].qDes(0);
    spiCommand->q_des_hip[leg] = commands[leg].qDes(1);
    spiCommand->q_des_knee[leg] = commands[leg].qDes(2);

    spiCommand->qd_des_abad[leg] = commands[leg].qdDes(0);
    spiCommand->qd_des_hip[leg] = commands[leg].qdDes(1);
    spiCommand->qd_des_knee[leg] = commands[leg].qdDes(2);

    // estimate torque
    datas[leg].tauEstimate =
        legTorque +
        commands[leg].kpJoint * (commands[leg].qDes - datas[leg].q) +
        commands[leg].kdJoint * (commands[leg].qdDes - datas[leg].qd);

    spiCommand->flags[leg] = _legsEnabled ? 1 : 0;
  }
}

Here is the caller graph for this function:

template<typename T >

void LegController< T >::updateCommand

(

TiBoardCommand *

tiBoardCommand

)

Update the "leg command" for the TI Board

Definition at line 180 of file LegController.cpp.

References TiBoardCommand::enable, TiBoardCommand::force_ff, TiBoardCommand::kd, TiBoardCommand::kp, TiBoardCommand::max_torque, TiBoardCommand::position_des, TiBoardCommand::tau_ff, and TiBoardCommand::velocity_des.

                                                                   {
  for (int leg = 0; leg < 4; leg++) {
    Vec3<T> tauFF = commands[leg].tauFeedForward.template cast<T>();
    tauFF += commands[leg].kpJoint * (commands[leg].qDes - datas[leg].q) +
             commands[leg].kdJoint * (commands[leg].qdDes - datas[leg].qd);

    for (int joint = 0; joint < 3; joint++) {
      tiBoardCommand[leg].kp[joint] = commands[leg].kpCartesian(joint, joint);
      tiBoardCommand[leg].kd[joint] = commands[leg].kdCartesian(joint, joint);
      tiBoardCommand[leg].tau_ff[joint] = tauFF[joint];
      tiBoardCommand[leg].position_des[joint] = commands[leg].pDes[joint];
      tiBoardCommand[leg].velocity_des[joint] = commands[leg].vDes[joint];
      tiBoardCommand[leg].force_ff[joint] =
          commands[leg].forceFeedForward[joint];
    }

    tiBoardCommand[leg].enable = _legsEnabled ? 1 : 0;
    tiBoardCommand[leg].max_torque = _maxTorque;
  }
}

template<typename T >

void LegController< T >::updateData

(

const SpiData *

spiData

)

Update the "leg data" from a SPIne board message

Definition at line 83 of file LegController.cpp.

References SpiData::q_abad, SpiData::q_hip, SpiData::q_knee, SpiData::qd_abad, SpiData::qd_hip, and SpiData::qd_knee.

                                                        {
  for (int leg = 0; leg < 4; leg++) {
    // q:
    datas[leg].q(0) = spiData->q_abad[leg];
    datas[leg].q(1) = spiData->q_hip[leg];
    datas[leg].q(2) = spiData->q_knee[leg];

    // qd
    datas[leg].qd(0) = spiData->qd_abad[leg];
    datas[leg].qd(1) = spiData->qd_hip[leg];
    datas[leg].qd(2) = spiData->qd_knee[leg];

    // J and p
    computeLegJacobianAndPosition<T>(_quadruped, datas[leg].q, &(datas[leg].J),
                                     &(datas[leg].p), leg);

    // v
    datas[leg].v = datas[leg].J * datas[leg].qd;
  }
}

Here is the caller graph for this function:

template<typename T >

void LegController< T >::updateData

(

const TiBoardData *

tiBoardData

)

Update the "leg data" from a TI Board message

Definition at line 108 of file LegController.cpp.

References TiBoardData::dq, TiBoardData::position, TiBoardData::q, TiBoardData::tau, and TiBoardData::velocity.

                                                                {
  for (int leg = 0; leg < 4; leg++) {
    for (int joint = 0; joint < 3; joint++) {
      datas[leg].q(joint) = tiBoardData[leg].q[joint];
      datas[leg].qd(joint) = tiBoardData[leg].dq[joint];
      datas[leg].p(joint) = tiBoardData[leg].position[joint];
      datas[leg].v(joint) = tiBoardData[leg].velocity[joint];
      computeLegJacobianAndPosition<T>(_quadruped, datas[leg].q, &datas[leg].J,
                                       nullptr, leg);
      datas[leg].tauEstimate[joint] = tiBoardData[leg].tau[joint];
    }
  }
}

template<typename T >

void LegController< T >::zeroCommand

(

)

Zero all leg commands. This should be run before any control code, so if the control code is confused and doesn't change the leg command, the legs won't remember the last command.

Definition at line 48 of file LegController.cpp.

                                   {
  for (auto& cmd : commands) {
    cmd.zero();
  }
  _legsEnabled = false;
}

Here is the caller graph for this function:

Member Data Documentation

template<typename T>

bool LegController< T >::_legsEnabled = false

Definition at line 72 of file LegController.h.

template<typename T>

T LegController< T >::_maxTorque = 0

Definition at line 73 of file LegController.h.

template<typename T>

Quadruped<T>& LegController< T >::_quadruped

Definition at line 71 of file LegController.h.

template<typename T>

LegControllerCommand<T> LegController< T >::commands[4]

Definition at line 69 of file LegController.h.

template<typename T>

LegControllerData<T> LegController< T >::datas[4]

Definition at line 70 of file LegController.h.

---

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

• LegController.h
• LegController.cpp