Skip to content

Commit

Permalink
feat: compute total inertia tensor
Browse files Browse the repository at this point in the history
  • Loading branch information
@HiroIshida
HiroIshida committed Jun 2, 2024
1 parent 2bcf8ac commit 298a0d9
Show file tree
Hide file tree
Showing 4 changed files with 260 additions and 1 deletion.
184 changes: 184 additions & 0 deletions data/kuka.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,184 @@
<?xml version="1.0" ?>
<!-- ======================================================================= -->
<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- | Changes (kohlhoff): | -->
<!-- | * Removed gazebo tags. | -->
<!-- | * Removed unused materials. | -->
<!-- | * Made mesh paths relative. | -->
<!-- | * Removed material fields from collision segments. | -->
<!-- | * Removed the self_collision_checking segment. | -->
<!-- | * Removed transmission segments, since they didn't match the | -->
<!-- | convention, will have to added back later. | -->
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
<!--Orebro University, Sweden -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions of source code must retain the above copyright notice,-->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. Redistributions in binary form must reproduce the above copyright -->
<!-- notice, this list of conditions and the following disclaimer in the -->
<!-- documentation and/or other materials provided with the distribution. -->
<!-- -->
<!--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -->
<!--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,-->
<!--THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -->
<!--PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -->
<!--CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -->
<!--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -->
<!--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -->
<!--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -->
<!--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -->
<!--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -->
<!--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Import Rviz colors -->
<material name="Grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="Orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="Blue">
<color rgba="0.5 0.7 1.0 1.0"/>
</material>

<!--Import the lbr iiwa macro -->
<!--Import Transmissions -->
<!--Include Utilities -->
<!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
<!--Little helper macros to define the inertia matrix needed for links.-->
<!--Cuboid-->
<!--Cylinder: length is along the y-axis! -->
<!--lbr-->
<link name="lbr_iiwa_link_0">
<inertial>
<origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
<!--Increase mass from 5 Kg original to provide a stable base to carry the
arm.-->
<mass value="0.0"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
</inertial>
</link>
<!-- joint between link_0 and link_1 -->
<joint name="lbr_iiwa_joint_1" type="revolute">
<parent link="lbr_iiwa_link_0"/>
<child link="lbr_iiwa_link_1"/>
<origin rpy="0 0 0" xyz="0 0 0.1575"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_1">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
<mass value="4"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
</link>
<!-- joint between link_1 and link_2 -->
<joint name="lbr_iiwa_joint_2" type="revolute">
<parent link="lbr_iiwa_link_1"/>
<child link="lbr_iiwa_link_2"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_2">
<inertial>
<origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
<mass value="4"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
</inertial>
</link>
<!-- joint between link_2 and link_3 -->
<joint name="lbr_iiwa_joint_3" type="revolute">
<parent link="lbr_iiwa_link_2"/>
<child link="lbr_iiwa_link_3"/>
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_3">
<inertial>
<origin rpy="0 0 0" xyz="0 0.03 0.13"/>
<mass value="3"/>
<inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- joint between link_3 and link_4 -->
<joint name="lbr_iiwa_joint_4" type="revolute">
<parent link="lbr_iiwa_link_3"/>
<child link="lbr_iiwa_link_4"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_4">
<inertial>
<origin rpy="0 0 0" xyz="0 0.067 0.034"/>
<mass value="2.7"/>
<inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
</inertial>
</link>
<!-- joint between link_4 and link_5 -->
<joint name="lbr_iiwa_joint_5" type="revolute">
<parent link="lbr_iiwa_link_4"/>
<child link="lbr_iiwa_link_5"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_5">
<inertial>
<origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
<mass value="1.7"/>
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
</inertial>
</link>
<!-- joint between link_5 and link_6 -->
<joint name="lbr_iiwa_joint_6" type="revolute">
<parent link="lbr_iiwa_link_5"/>
<child link="lbr_iiwa_link_6"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_6">
<inertial>
<origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
<mass value="1.8"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
</inertial>
</link>
<!-- joint between link_6 and link_7 -->
<joint name="lbr_iiwa_joint_7" type="revolute">
<parent link="lbr_iiwa_link_6"/>
<child link="lbr_iiwa_link_7"/>
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
<dynamics damping="0.5"/>
</joint>
<link name="lbr_iiwa_link_7">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.02"/>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>

</robot>
2 changes: 2 additions & 0 deletions include/tinyfk.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,8 @@ class KinematicModel {
Eigen::MatrixXd get_com_jacobian(const std::vector<size_t> &joint_ids,
bool with_base);

Eigen::Matrix3d get_total_inertia_matrix();

void set_joint_angle(size_t joint_id, double angle) {
joint_angles_[joint_id] = angle;
}
Expand Down
47 changes: 47 additions & 0 deletions src/kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,4 +271,51 @@ KinematicModel::get_com_jacobian(const std::vector<size_t> &joint_ids,
return jac_average;
}

Eigen::Matrix3d KinematicModel::get_total_inertia_matrix() {
auto com = this->get_com();

Eigen::Matrix3d Imat_total = Eigen::Matrix3d::Zero();
for (const auto &link : com_dummy_links_) {
const auto inertial = link->inertial;
if (inertial != nullptr) {
double mass = inertial->mass;
double ixx = inertial->ixx;
double iyy = inertial->iyy;
double izz = inertial->izz;
double ixy = inertial->ixy;
double ixz = inertial->ixz;
double iyz = inertial->iyz;
Eigen::Matrix3d Imat;
Imat << ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz;
size_t link_id = link->id;

urdf::Pose tf_base_to_link;
this->get_link_pose(link_id, tf_base_to_link);
const auto &trans = tf_base_to_link.position;
Eigen::Vector3d vec;
vec << trans.x - com.x, trans.y - com.y, trans.z - com.z;
const auto &rot = tf_base_to_link.rotation;
double xy2 = 2 * (rot.x * rot.y);
double xz2 = 2 * (rot.x * rot.z);
double xw2 = 2 * (rot.x * rot.w);
double yz2 = 2 * (rot.y * rot.z);
double yw2 = 2 * (rot.y * rot.w);
double zw2 = 2 * (rot.z * rot.w);
double xx2 = 2 * (rot.x * rot.x);
double yy2 = 2 * (rot.y * rot.y);
double zz2 = 2 * (rot.z * rot.z);

Eigen::Matrix3d R;
R << 1 - yy2 - zz2, xy2 - zw2, xz2 + yw2, xy2 + zw2, 1 - xx2 - zz2,
yz2 - xw2, xz2 - yw2, yz2 + xw2, 1 - xx2 - yy2;

Eigen::Matrix3d trans_term =
mass * (vec.norm() * vec.norm() * Eigen::Matrix3d::Identity() -
vec * vec.transpose());
Imat_total += (R * Imat * R.transpose() + trans_term);
}
}
return Imat_total;
}

}; // namespace tinyfk
28 changes: 27 additions & 1 deletion test/test_kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ Eigen::MatrixXd compute_numerical_jacobian_with_base(
return J;
}

TEST(KINEMATICS, AllTest) {
TEST(FORWARD_KINEMATICS_TEST, AllTest) {
// loading test data
ifstream test_data("../test/data/test_data.json");
nlohmann::json js;
Expand Down Expand Up @@ -195,6 +195,32 @@ TEST(KINEMATICS, AllTest) {
}
}

TEST(TOTAL_INERTIA_TEST, AllTest) {
// loading test data
const std::string urdf_file = "../data/kuka.urdf";
const auto xml_string = load_urdf(urdf_file);
auto kin = KinematicModel(xml_string);

const auto imat = kin.get_total_inertia_matrix();
std::cout << imat << std::endl;

// The following python code is used to generate the reference data
// import numpy as np
// import pinocchio as pin
// robot = pin.RobotWrapper.BuildFromURDF(
// filename="./data/kuka.urdf",
// package_dirs=None,
// root_joint=pin.JointModelFreeFlyer())
// q = np.zeros(robot.nq)
// Ag = pin.computeCentroidalMap(robot.model, robot.data, q)
// print(Ag[3:6, 3:6])
Eigen::Matrix3d imat_ref;
imat_ref << 2.11765803e+00, 4.59516343e-05, -2.15483177e-04, 4.59516343e-05,
2.08613831e+00, 2.17846493e-02, -2.15483177e-04, 2.17846493e-02,
1.14820264e-01;
EXPECT_TRUE((imat - imat_ref).array().abs().maxCoeff() < 1e-5);
}

int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Expand Down

0 comments on commit 298a0d9

Please sign in to comment.