Path planning and tracking methods for

Quadruped robots
Student Name: Mr. Vedang Tiwari
SURGE No.: 2130308
Guide Name: Dr. Mangal Kothari
Date: 01-08-2021

Abstract: Mobile terrestrial robots are traditionally designed to move on flat terrain, and the
mapping, planning, and control algorithms are typically developed for a two-dimensional abstraction
of the environment. When it comes to navigation on rough terrain (e.g., with tracked vehicles or
legged robots), these algorithms must be extended to consider all three dimensions of the
surroundings. The most common approach is to build an elevation map of the environment, where
each coordinate on the horizontal plane is associated with an elevation or height value. We are using
the Universal Grid Map Library as a generic mapping framework with the Robot Operating System
(ROS) for the quadruped robot. The library supports multiple data layers, such as assigning elevation,
variance, color, surface normal, occupancy, etc. They can produce a probabilistic representation of
the environment. The mapping procedure is formulated from a robot-centric perspective to
explicitly account for pose drift as the robot moves.

Keywords: elevation mapping, Grid Map Library, Robot Operating System (ROS), proprioceptive
sensors, exteroceptive sensors, state estimation, Extended Kalman Filter (EKF), kinematics, and
dynamics libraries.

1. Introduction
A few decades before, robots were a thing straight out of science fiction. In these stories, we saw
them sometimes as human beings' greatest enemy, but most of the time as our best companions.
And often picture them as a humanoid or quadruped robot (in general a legged robot), not only
because they are closer to natural physiology but also because they show superiority in rough and
uncertain terrain over wheeled robot tracked systems could even transverse discontinuous paths.
Unlike aerial robots, they are in contact with the ground, thus carrying much heavier loads. They
have many potentials and could be used as a domestic pet or for defense and rescue purposes.
Legged robots could perform highly dynamic maneuvers, thus requires sophisticated control
strategies to maintain balance and generating appropriate feet and body trajectories for accuracy. A
quadruped is less sophisticated than a hexapod (inspired by insects) because fewer legs are deployed
and more stable than a biped (humanoid robot) because of increased supported area, thus best
suited for most jobs. Still, a highly developed relationship between perception and locomotion is
necessary for autonomous navigation without falling, slipping, or getting stuck.
To use these robots in natural surroundings outside the laboratories, they must have reactive
capabilities, which implies handling unforeseen situations such as dust, sunlight, slipping, or external
impulses. This demands continuously adapting the generated motion plans with the incoming data
from the sensors such as IMU, Depth sensors, contact or torque sensors, stereo cameras, and LiDAR
to generate a probabilistic map of the environment continuously updated with minimum possible
latency or time delay.
 2. Literature Review
Not much research is done in this field specific to the quadruped robot, with only a few scientists like
Marco Camurri, Simona Nobili, Peter Fankhauser managed to establish their work. This section has
reviewed two state-of-the-art methods to endow a legged robot with motion estimation and
mapping capabilities. We have seen that legged machines can benefit from multi-sensor fusion
techniques, primarily when employed in complex operations in GPS-denied environments. The trend
towards methods that use a combination of proprioceptive and exteroceptive state estimation is
highlighted in this table.

a) Elevation Mapping
How to acquire and merge geometrical information about the environment in a compact,
consistent, and helpful representation. We are using the Universal Grid Map Library created
by Peter Fankhauser and Marco Hutter. The proposed method incorporates the drift and
uncertainties of the state estimation and a noise model of the distance sensor. It yields a
probabilistic terrain estimate as a grid-based elevation map, including upper and lower
confidence bounds. Grid maps can be thought of as a 2.5-dimensional representation, where
each cell in the grid holds a height value. They have implemented the elevation mapping
software on the quadrupedal robot StarlETH.

b) State Estimation
how to compute (efficiently, robustly, and at high frequency) an estimate of the robot's base's
position, orientation, and linear and angular velocity. When executed with only proprioceptive
inputs, drift in place is unbounded, and additional exteroceptive sources are required to
accommodate for it. The pose drift is limited using exteroceptive sensors like cameras, Light
Detection and Ranging (LiDAR), or depth sensors. We are using a multi-sensor state estimator
called Pronto by Marco Camurri and Simona Nobili. This is among the most recent algorithms
based on Extended Kalman Filter (EKF) that fuses IMU and Leg Odometry sensing for pose and
 velocity estimation and can integrate pose corrections from visual and LIDAR and Odometry to
correct pose drift. It has been tested on IIT HyQ and ANYbotics ANYmal.

3. System Overview
A standard quadruped model consists of 3 degrees of freedom (DoF) in each leg and six unactuated
DoF for the torso, making it a total of 18 DoF. Below is the xacro file of our quadruped robot.

Link arrangement of shvan quadruped

Robot Description:

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro" name="shvan">

<xacro:property name="hip_len" value="0.08" />
<xacro:property name="thigh_len" value="0.2" />
<xacro:property name="shank_len" value="0.23" />
<xacro:property name="link_radius" value="0.025"/>
<xacro:property name="foot_radius" value="0.0025"/>
<xacro:property name="body_len" value="0.6" />
<xacro:property name="body_width" value="0.42" />
<xacro:property name="body_height" value="0.1" />
<xacro:property name="body_mass" value="10.0" />
<xacro:property name="hip_mass" value="0.6" />
<xacro:property name="thigh_mass" value="1.2" />
<xacro:property name="shank_mass" value="0.3" />

<xacro:include filename="inertial.xacro" />

<!-- <material name="red">

<color rgba="1 0 0 1"/>
</material>

<material name="white">
<color rgba="1 1 1 1"/>
</material> -->

<pose>0 0 0.4 0 0 0</pose>

<link name="base_link">
<xacro:cuboid_interial mass="${body_mass}" x="0" y="0" z="0" roll="0
" pitch="0" yaw="0" lx="${body_len}" ly="${body_width}" lz="${body_height}"
/>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh
filename="model://shvan/meshes/base_link_torso.STL" />
</geometry>
<material name="">
<color rgba="1 0.95686 0.93725 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
 <mesh
filename="model://shvan/meshes/base_link_torso.STL" />
</geometry>
</collision>
</link>

<xacro:macro name="leg" params="suffix front left">

<link name="hip_link_${suffix}">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh
filename="model://shvan/meshes/leg_fr_hip.STL" />
</geometry>
<material name="">
<color rgba="0.66667 0.69804 0.76863 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh
filename="model://shvan/meshes/leg_fr_hip.STL" />
</geometry>
</collision>
<!--
<xacro:cylinder_interial mass="${hip_mass}" x="${hip_len / 2}" y="0"
z="0" roll="0" pitch="0" yaw="0" h="${hip_len}" r="${link_radius}" />
-->
<xacro:cuboid_interial mass="${hip_mass}" x="${hip_len / 2}" y="0" z
="0" roll="0" pitch="0" yaw="0" lx="${hip_len}" ly="${link_radius}" lz="${li
nk_radius}" />
<!--
<xacro:default_inertial mass="2"/>
-->
</link>
<joint name="hip_joint_${suffix}" type="revolute">
<axis xyz="0 0 1" initial_position="0"/>
<limit effort="100.0" lower="${-
0.5 * pi}" upper="${0.5 * pi}" velocity="100"/>
<parent link="base_link"/>
<child link="hip_link_${suffix}"/>
<origin xyz="${0.5*body_len*front} ${left*0.5*body_width} 0" rpy
= "${0.5*pi} ${0.5*pi} ${0.5*pi}"/>
</joint>
Part of Shvan quadruped robot's xacro file (like hip link other links are also defined)
Laptop specifications: Ubuntu 20.04, ROS noetic, Intel i5 8th gen processor with 8GB RAM and 4GB
NVIDIA Graphic card

4. Approach
After analyzing different onboard sensor suites, we choose to include contact or torque sensors,
joint encoders, IMUs, depth sensor (RGB-D sensor), stereo camera, and LiDAR on our robot with
optional intermittent GPS position updates.

Block diagram of the system. These modules output filter measurements as ROS messages.

The range measurement data we get for elevation mapping through the universal grid map library
from Kinect-type sensors, laser range sensors, and stereo cameras. The node subscribes to
PointCloud2 depth measurements and a robot pose estimate of type PoseWithCovarianceStamped.
For pose estimation, we are using the Pronto state estimator. To integrate your own Visual and
LIDAR Odometry packages, make them produce the same message type that the fovis (the FOVIS
visual odometry algorithm) will expect them to produce.

Finally, we could plan the path with an RRT-based path planner after the elevation map is generated.

Implementation of the RRT algorithm for unknown environment

 5. Conclusion
We have managed to generate kinematic and dynamic libraries of the robot completing half of the
requirements for state estimation. Further, we need to plugin sensors on our model in gazebo and
publish messages of specific types. After adding those topics to the nodes of pronto and elevation
mapping packages, we could generate the probabilistic elevation maps for the surrounding.

Image showing links and Joints of the robot

// + Link shank_link_RH
shank_link_RH_u = tau(KNEE_JOINT_RH) - shank_link_RH_p(AZ);
shank_link_RH_U = shank_link_RH_AI.col(AZ);
shank_link_RH_D = shank_link_RH_U(AZ);

compute_Ia_revolute(shank_link_RH_AI, shank_link_RH_U, shank_link_RH_D,

Ia_r); // same as: Ia_r = shank_link_RH_AI - shank_link_RH_U/shank_link_RH_
D * shank_link_RH_U.transpose();
pa = shank_link_RH_p + Ia_r * shank_link_RH_c + shank_link_RH_U * shank_
link_RH_u/shank_link_RH_D;
ctransform_Ia_revolute(Ia_r, motionTransforms-
> fr_shank_link_RH_X_fr_thigh_link_RH, IaB);
thigh_link_RH_AI += IaB;
thigh_link_RH_p += (motionTransforms-
> fr_shank_link_RH_X_fr_thigh_link_RH).transpose() * pa;

com_base = Vector3(0.0,0.0,0.0);
tensor_base.fill(
m_base,
com_base,
Utils::buildInertiaTensor<Scalar>(ix_base,iy_base,iz_base,0.0,0.0,0.
0) );
// First pass, link 'hip_link_LF'
hip_link_LF_v = ((xm->fr_hip_link_LF_X_fr_base) * base_v);
hip_link_LF_v(iit::rbd::AZ) += qd(HIP_JOINT_LF);

motionCrossProductMx<Scalar>(hip_link_LF_v, vcross);

hip_link_LF_a = (xm-
>fr_hip_link_LF_X_fr_base) * base_a + vcross.col(iit::rbd::AZ) * qd(HIP_JOIN
T_LF);
hip_link_LF_a(iit::rbd::AZ) += qdd(HIP_JOINT_LF);

hip_link_LF_f = hip_link_LF_I * hip_link_LF_a + vxIv(hip_link_LF_v, hip_

link_LF_I) - fext[HIP_LINK_LF];

void rcg::shvan1::Jacobians::updateParameters(const Params_lengths& _length

s, const Params_angles& _angles)
{
params.lengths = _lengths;
params.angles = _angles;
params.trig.update();
}

rcg::shvan1::Jacobians::Type_fr_base_J_LF_FOOT::Type_fr_base_J_LF_FOOT()
{
(*this)(0,0) = 1.0;
(*this)(0,1) = 0.0;
(*this)(0,2) = 0.0;
(*this)(1,0) = 0.0;
(*this)(2,0) = 0.0;
(*this)(3,0) = 0.0;
}

const rcg::shvan1::Jacobians::Type_fr_base_J_LF_FOOT& rcg::shvan1::Jacobians

::Type_fr_base_J_LF_FOOT::update(const JointState& q)
{
Scalar sin_q_hip_joint_LF = ScalarTraits::sin( q(HIP_JOINT_LF) );
Scalar cos_q_hip_joint_LF = ScalarTraits::cos( q(HIP_JOINT_LF) );
Scalar sin_q_elbow_joint_LF = ScalarTraits::sin( q(ELBOW_JOINT_LF) );
Scalar cos_q_elbow_joint_LF = ScalarTraits::cos( q(ELBOW_JOINT_LF) );
Scalar sin_q_knee_joint_LF = ScalarTraits::sin( q(KNEE_JOINT_LF) );
Scalar cos_q_knee_joint_LF = ScalarTraits::cos( q(KNEE_JOINT_LF) );
(*this)(1,1) = -cos_q_hip_joint_LF;
(*this)(1,2) = -cos_q_hip_joint_LF;
(*this)(2,1) = -sin_q_hip_joint_LF;
(*this)(2,2) = -sin_q_hip_joint_LF;
(*this)(3,1) = (- tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_knee_joint_L
F)+( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_knee_joint_LF)+( tx_knee_join
t_LF * cos_q_elbow_joint_LF);
 (*this)(3,2) = ( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_knee_joint_LF
)-( tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_knee_joint_LF);
(*this)(4,0) = (- tx_LF_FOOT * sin_q_elbow_joint_LF * cos_q_hip_joint_LF
* sin_q_knee_joint_LF)+( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_hip_join
t_LF * cos_q_knee_joint_LF)+((( tx_knee_joint_LF * cos_q_elbow_joint_LF)+ tx
_elbow_joint_LF) * cos_q_hip_joint_LF);
(*this)(4,1) = (- tx_LF_FOOT * cos_q_elbow_joint_LF * sin_q_hip_joint_LF
* sin_q_knee_joint_LF)-
( tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_joint_
LF)-( tx_knee_joint_LF * sin_q_elbow_joint_LF * sin_q_hip_joint_LF);
(*this)(4,2) = (- tx_LF_FOOT * cos_q_elbow_joint_LF * sin_q_hip_joint_LF
* sin_q_knee_joint_LF)-
( tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_joint_
LF);
(*this)(5,0) = (- tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_hip_joint_LF
* sin_q_knee_joint_LF)+( tx_LF_FOOT * cos_q_elbow_joint_LF * sin_q_hip_join
t_LF * cos_q_knee_joint_LF)+((( tx_knee_joint_LF * cos_q_elbow_joint_LF)+ tx
_elbow_joint_LF) * sin_q_hip_joint_LF);
(*this)(5,1) = ( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_hip_joint_LF
* sin_q_knee_joint_LF)+( tx_LF_FOOT * sin_q_elbow_joint_LF * cos_q_hip_joint
_LF * cos_q_knee_joint_LF)+( tx_knee_joint_LF * sin_q_elbow_joint_LF * cos_q
_hip_joint_LF);
(*this)(5,2) = ( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_hip_joint_LF
* sin_q_knee_joint_LF)+( tx_LF_FOOT * sin_q_elbow_joint_LF * cos_q_hip_joint
_LF * cos_q_knee_joint_LF);
return *this;

// Link shank_link_RH:
iit::rbd::transformInertia<Scalar>(shank_link_RH_Ic, frcTransf -
> fr_thigh_link_RH_X_fr_shank_link_RH, Ic_spare);
thigh_link_RH_Ic += Ic_spare;

Fcol(KNEE_JOINT_RH) = shank_link_RH_Ic.col(AZ);
DATA(KNEE_JOINT_RH+6, KNEE_JOINT_RH+6) = Fcol(KNEE_JOINT_RH)(AZ);

Fcol(KNEE_JOINT_RH) = frcTransf -
> fr_thigh_link_RH_X_fr_shank_link_RH * Fcol(KNEE_JOINT_RH);
DATA(KNEE_JOINT_RH+6, ELBOW_JOINT_RH+6) = F(AZ,KNEE_JOINT_RH);
DATA(ELBOW_JOINT_RH+6, KNEE_JOINT_RH+6) = DATA(KNEE_JOINT_RH+6, ELBOW_JO
INT_RH+6);
Fcol(KNEE_JOINT_RH) = frcTransf -
> fr_hip_link_RH_X_fr_thigh_link_RH * Fcol(KNEE_JOINT_RH);
DATA(KNEE_JOINT_RH+6, HIP_JOINT_RH+6) = F(AZ,KNEE_JOINT_RH);
DATA(HIP_JOINT_RH+6, KNEE_JOINT_RH+6) = DATA(KNEE_JOINT_RH+6, HIP_JOINT_
RH+6);
Fcol(KNEE_JOINT_RH) = frcTransf -
> fr_base_X_fr_hip_link_RH * Fcol(KNEE_JOINT_RH);
 base_X_hip_joint_LF_chain = tmpX * ht.fr_base_X_fr_hip_link_LF;
tmpSum += inertiaProps.getMass_hip_link_LF() *
( iit::rbd::Utils::transform(base_X_hip_joint_LF_chain, inertiaP
rops.getCOM_hip_link_LF()));

void HomogeneousTransforms::updateParams(const Params_lengths& v

_lengths, const Params_angles& v_angles)
{
params.lengths = v_lengths;
params.angles = v_angles;
params.trig.update();
}

MotionTransforms::Type_fr_base_X_LF_FOOT::Type_fr_base_X_LF_FOOT()
{
(*this)(0,2) = 0.0;
(*this)(0,3) = 0.0;
(*this)(0,4) = 0.0;
(*this)(0,5) = 0.0;
(*this)(1,3) = 0.0;
(*this)(1,4) = 0.0;
(*this)(1,5) = 0.0;
(*this)(2,3) = 0.0;
(*this)(2,4) = 0.0;
(*this)(2,5) = 0.0;
(*this)(3,5) = 0.0;
}

const MotionTransforms::Type_fr_base_X_LF_FOOT& MotionTransforms::Type_fr_ba

se_X_LF_FOOT::update(const state_t& q)
{
Scalar sin_q_hip_joint_LF = ScalarTraits::sin( q(HIP_JOINT_LF) );
Scalar cos_q_hip_joint_LF = ScalarTraits::cos( q(HIP_JOINT_LF) );
Scalar sin_q_elbow_joint_LF = ScalarTraits::sin( q(ELBOW_JOINT_LF) );
Scalar cos_q_elbow_joint_LF = ScalarTraits::cos( q(ELBOW_JOINT_LF) );
Scalar sin_q_knee_joint_LF = ScalarTraits::sin( q(KNEE_JOINT_LF) );
Scalar cos_q_knee_joint_LF = ScalarTraits::cos( q(KNEE_JOINT_LF) );
(*this)(0,0) = (cos_q_elbow_joint_LF * sin_q_knee_joint_LF)+(sin_q_elbow
_joint_LF * cos_q_knee_joint_LF);
(*this)(0,1) = (cos_q_elbow_joint_LF * cos_q_knee_joint_LF)-
(sin_q_elbow_joint_LF * sin_q_knee_joint_LF);
(*this)(1,0) = (cos_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_j
oint_LF)-(sin_q_elbow_joint_LF * sin_q_hip_joint_LF * sin_q_knee_joint_LF);
 (*this)(1,1) = (-
cos_q_elbow_joint_LF * sin_q_hip_joint_LF * sin_q_knee_joint_LF)-
(sin_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(1,2) = -cos_q_hip_joint_LF;
(*this)(2,0) = (sin_q_elbow_joint_LF * cos_q_hip_joint_LF * sin_q_knee_j
oint_LF)-(cos_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(2,1) = (cos_q_elbow_joint_LF * cos_q_hip_joint_LF * sin_q_knee_j
oint_LF)+(sin_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(2,2) = -sin_q_hip_joint_LF;
(*this)(3,0) = ( ty_hip_joint_LF * sin_q_elbow_joint_LF * cos_q_hip_join
t_LF * sin_q_knee_joint_LF)-
( ty_hip_joint_LF * cos_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_j
oint_LF);
(*this)(3,1) = ( ty_hip_joint_LF * cos_q_elbow_joint_LF * cos_q_hip_join
t_LF * sin_q_knee_joint_LF)+( ty_hip_joint_LF * sin_q_elbow_joint_LF * cos_q
_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(3,2) = ( tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_knee_joint_LF
)-( tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_knee_joint_LF)-
( ty_hip_joint_LF * sin_q_hip_joint_LF)-
( tx_knee_joint_LF * cos_q_elbow_joint_LF)- tx_elbow_joint_LF;
(*this)(3,3) = (cos_q_elbow_joint_LF * sin_q_knee_joint_LF)+(sin_q_elbow
_joint_LF * cos_q_knee_joint_LF);
(*this)(3,4) = (cos_q_elbow_joint_LF * cos_q_knee_joint_LF)-
(sin_q_elbow_joint_LF * sin_q_knee_joint_LF);
(*this)(4,0) = (((- tx_hip_joint_LF * sin_q_elbow_joint_LF)-
( tx_elbow_joint_LF * cos_q_elbow_joint_LF)- tx_knee_joint_LF) * cos_q_hip_j
oint_LF * sin_q_knee_joint_LF)+((( tx_hip_joint_LF * cos_q_elbow_joint_LF)-
( tx_elbow_joint_LF * sin_q_elbow_joint_LF)) * cos_q_hip_joint_LF * cos_q_kn
ee_joint_LF);
(*this)(4,1) = ((( tx_elbow_joint_LF * sin_q_elbow_joint_LF)-
( tx_hip_joint_LF * cos_q_elbow_joint_LF)) * cos_q_hip_joint_LF * sin_q_knee
_joint_LF)+(((- tx_hip_joint_LF * sin_q_elbow_joint_LF)-
( tx_elbow_joint_LF * cos_q_elbow_joint_LF)- tx_knee_joint_LF) * cos_q_hip_j
oint_LF * cos_q_knee_joint_LF)-( tx_LF_FOOT * cos_q_hip_joint_LF);
(*this)(4,2) = ( tx_LF_FOOT * cos_q_elbow_joint_LF * sin_q_hip_joint_LF
* sin_q_knee_joint_LF)+( tx_LF_FOOT * sin_q_elbow_joint_LF * sin_q_hip_joint
_LF * cos_q_knee_joint_LF)+((( tx_knee_joint_LF * sin_q_elbow_joint_LF)+ tx_
hip_joint_LF) * sin_q_hip_joint_LF);
(*this)(4,3) = (cos_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_j
oint_LF)-(sin_q_elbow_joint_LF * sin_q_hip_joint_LF * sin_q_knee_joint_LF);
(*this)(4,4) = (-
cos_q_elbow_joint_LF * sin_q_hip_joint_LF * sin_q_knee_joint_LF)-
(sin_q_elbow_joint_LF * sin_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(4,5) = -cos_q_hip_joint_LF;
(*this)(5,0) = (((((- tx_hip_joint_LF * sin_q_elbow_joint_LF)-
( tx_elbow_joint_LF * cos_q_elbow_joint_LF)- tx_knee_joint_LF) * sin_q_hip_j
oint_LF)-
( ty_hip_joint_LF * cos_q_elbow_joint_LF)) * sin_q_knee_joint_LF)+((((( tx_h
 ip_joint_LF * cos_q_elbow_joint_LF)-
( tx_elbow_joint_LF * sin_q_elbow_joint_LF)) * sin_q_hip_joint_LF)-
( ty_hip_joint_LF * sin_q_elbow_joint_LF)) * cos_q_knee_joint_LF);
(*this)(5,1) = ((((( tx_elbow_joint_LF * sin_q_elbow_joint_LF)-
( tx_hip_joint_LF * cos_q_elbow_joint_LF)) * sin_q_hip_joint_LF)+( ty_hip_jo
int_LF * sin_q_elbow_joint_LF)) * sin_q_knee_joint_LF)+(((((- tx_hip_joint_L
F * sin_q_elbow_joint_LF)-
( tx_elbow_joint_LF * cos_q_elbow_joint_LF)- tx_knee_joint_LF) * sin_q_hip_j
oint_LF)-( ty_hip_joint_LF * cos_q_elbow_joint_LF)) * cos_q_knee_joint_LF)-
( tx_LF_FOOT * sin_q_hip_joint_LF);
(*this)(5,2) = (- tx_LF_FOOT * cos_q_elbow_joint_LF * cos_q_hip_joint_LF
* sin_q_knee_joint_LF)-
( tx_LF_FOOT * sin_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_joint_
LF)+(((- tx_knee_joint_LF * sin_q_elbow_joint_LF)- tx_hip_joint_LF) * cos_q_
hip_joint_LF);
(*this)(5,3) = (sin_q_elbow_joint_LF * cos_q_hip_joint_LF * sin_q_knee_j
oint_LF)-(cos_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(5,4) = (cos_q_elbow_joint_LF * cos_q_hip_joint_LF * sin_q_knee_j
oint_LF)+(sin_q_elbow_joint_LF * cos_q_hip_joint_LF * cos_q_knee_joint_LF);
(*this)(5,5) = -sin_q_hip_joint_LF;
return *this;
}

These are sections of the generated code for only one link giving us the jacobians, inertia
properties, forward and inverse dynamics, and transformation matrixes

GitHub link for above code: HTTPS ://GITHUB.COM/VEDANGT/QUADRUPED

References:

1. HTTPS :// WWW.FRONTIERSIN. ORG/ARTICLES/10.3389/ FROBT.2020.00068/ FULL

2. HTTPS :// WWW.RESEARCHGATE .NET/PUBLICATION/318789245_H ETEROGENEOUS_SENSOR_FU
SION_FOR_ACCURATE _S TATE _ESTIMATION _OF_DYNAMIC _L EGGED_ROBOTS
3. HTTPS :// JOURNALS.SAGEPUB .COM/DOI/ABS/10.1177/0278364915587333? RELATED-
URLS=YES&LEGID=SPIJR%3B0278364915587333 V1
4. HTTPS :// WWW.RESEARCHGATE .NET/PUBLICATION/243971940_S TATE_ESTIMATION_FOR_LEG
GED_ROBOTS_-_C ONSISTENT _FUSION_OF_LEG_KINEMATICS _AND_IMU
5. HTTPS :// IIT-DLSLAB.GITHUB.IO/PAPERS/CAMURRI17PHD.PDF
6. HTTPS :// WWW.RESEARCHGATE .NET/PUBLICATION/325918227_P ROBABILISTIC _TERRAIN_MAP
PING _FOR_MOBILE _ROBOTS_WITH_UNCERTAIN_LOCALIZATION / CITATIONS
7. HTTPS :// WWW.RESEARCHGATE .NET/PUBLICATION/284415855_A_U NIVERSAL _GRID_MAP_LI
BRARY_I MPLEMENTATION _AND_USE _C ASE _FOR_ROUGH _T ERRAIN_N AVIGATION
8. HTTPS :// GITHUB.COM/ORI-DRS/PRONTO
9. HTTPS :// GITHUB.COM/ORI-DRS/QUADRUPED_ROBCOGEN
10. HTTPS :// GITHUB.COM/ANYBOTICS/ELEVATION_MAPPING