LegController.h File Reference

Common Leg Control Interface and Leg Control Algorithms. More...

#include <eigen3/Eigen/Dense>
#include "leg_control_command_lcmt.hpp"
#include "leg_control_data_lcmt.hpp"
#include "Dynamics/Quadruped.h"
#include "SimUtilities/SpineBoard.h"
#include "SimUtilities/ti_boardcontrol.h"
#include "cppTypes.h"

Include dependency graph for LegController.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	LegControllerCommand< T >
struct	LegControllerData< T >
class	LegController< T >

Functions
template<typename T >
void	computeLegJacobianAndPosition (Quadruped< T > &quad, Vec3< T > &q, Mat3< T > *J, Vec3< T > *p, int leg)

Detailed Description

Common Leg Control Interface and Leg Control Algorithms.

Implements low-level leg control for Mini Cheetah and Cheetah 3 Robots Abstracts away the difference between the SPIne and the TI Boards All quantities are in the "leg frame" which has the same orientation as the body frame, but is shifted so that 0,0,0 is at the ab/ad pivot (the "hip frame").

Definition in file LegController.h.

Function Documentation

template<typename T >

void computeLegJacobianAndPosition	(	Quadruped< T > &	quad,
		Vec3< T > &	q,
		Mat3< T > *	J,
		Vec3< T > *	p,
		int	leg
	)

Compute the position of the foot and its Jacobian. This is done in the local leg coordinate system. If J/p are NULL, the calculation will be skipped.

Definition at line 240 of file LegController.cpp.

References Quadruped< T >::_abadLinkLength, Quadruped< T >::_hipLinkLength, Quadruped< T >::_kneeLinkLength, computeLegJacobianAndPosition< double >(), computeLegJacobianAndPosition< float >(), and Quadruped< T >::getSideSign().

                                                        {
  T l1 = quad._abadLinkLength;
  T l2 = quad._hipLinkLength;
  T l3 = quad._kneeLinkLength;
  T sideSign = quad.getSideSign(leg);

  T s1 = std::sin(q(0));
  T s2 = std::sin(q(1));
  T s3 = std::sin(q(2));

  T c1 = std::cos(q(0));
  T c2 = std::cos(q(1));
  T c3 = std::cos(q(2));

  T c23 = c2 * c3 - s2 * s3;
  T s23 = s2 * c3 + c2 * s3;

  if (J) {
    J->operator()(0, 0) = 0;
    J->operator()(0, 1) = l3 * c23 + l2 * c2;
    J->operator()(0, 2) = l3 * c23;
    J->operator()(1, 0) = l3 * c1 * c23 + l2 * c1 * c2 - l1 * sideSign * s1;
    J->operator()(1, 1) = -l3 * s1 * s23 - l2 * s1 * s2;
    J->operator()(1, 2) = -l3 * s1 * s23;
    J->operator()(2, 0) = l3 * s1 * c23 + l2 * c2 * s1 + l1 * sideSign * c1;
    J->operator()(2, 1) = l3 * c1 * s23 + l2 * c1 * s2;
    J->operator()(2, 2) = l3 * c1 * s23;
  }

  if (p) {
    p->operator()(0) = l3 * s23 + l2 * s2;
    p->operator()(1) = l1 * sideSign * c1 + l3 * (s1 * c23) + l2 * c2 * s1;
    p->operator()(2) = l1 * sideSign * s1 - l3 * (c1 * c23) - l2 * c1 * c2;
  }
}

Here is the call graph for this function:

Here is the caller graph for this function: