LegController.cpp File Reference

Common Leg Control Interface. More...

#include <eigen3/Eigen/Dense>
#include "Controllers/LegController.h"

Include dependency graph for LegController.cpp:

Go to the source code of this file.

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

Detailed Description

Common Leg Control Interface.

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.cpp.

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:

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

Here is the caller graph for this function:

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

Here is the caller graph for this function: