Skip to content

Commit

Permalink
Add kinematics_ros_utils base files
Browse files Browse the repository at this point in the history
  • Loading branch information
ShotaAk committed Nov 17, 2023
1 parent c101965 commit 48f8b19
Show file tree
Hide file tree
Showing 6 changed files with 517 additions and 0 deletions.
19 changes: 19 additions & 0 deletions rt_manipulators_lib/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ add_library(${library_name}
src/hardware_joints.cpp
src/hardware_communicator.cpp
src/kinematics.cpp
src/kinematics_ros_utils.cpp
src/kinematics_utils.cpp
src/config_file_parser.cpp
src/dynamixel_x.cpp
Expand Down Expand Up @@ -64,4 +65,22 @@ install(
INCLUDES DESTINATION include
)

if(BUILD_TESTING)
find_package(ament_cmake_gtest)
ament_add_gtest(test_kinematics_ros_utils
ros_test/test_kinematics_ros_utils.cpp)

target_link_libraries(${test_kinematics_ros_utils}
${library_name}
)
target_include_directories(test_kinematics_ros_utils PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)

install(DIRECTORY
ros_test/urdf
DESTINATION share/${PROJECT_NAME}/
)
endif()

ament_package()
28 changes: 28 additions & 0 deletions rt_manipulators_lib/include/kinematics_ros_utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
// Copyright 2023 RT Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_ROS_UTILS_HPP_
#define RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_ROS_UTILS_HPP_

#include <string>

#include "kinematics_utils.hpp"

namespace kinematics_ros_utils {

kinematics_utils::links_t parse_urdf_file(const std::string & path);

} // kinematics_ros_utils

#endif // RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_ROS_UTILS_HPP_
34 changes: 34 additions & 0 deletions rt_manipulators_lib/ros_test/test_kinematics_ros_utils.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
// Copyright 2023 RT Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <vector>

#include "gtest/gtest.h"
#include "rt_manipulators_cpp/link.hpp"

class KinematicsROSUtilsFixture: public ::testing::Test {
protected:
virtual void SetUp() {
}

virtual void TearDown() {
}

std::vector<manipulators_link::Link> links;
};

TEST_F(KinematicsROSUtilsFixture, load_link_names) {
EXPECT_EQ("test_base", "test_base");
}
274 changes: 274 additions & 0 deletions rt_manipulators_lib/ros_test/urdf/test_robot.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,274 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from test_robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="bonobo">
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="r_leg_joint1" type="revolute">
<parent link="base_link"/>
<child link="r_leg_link1"/>
<origin rpy="0 0 0" xyz="0 -0.5 -0.5"/>
<axis xyz="0 1 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="r_leg_link1">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="r_leg_joint2" type="revolute">
<parent link="r_leg_link1"/>
<child link="r_leg_link2"/>
<origin rpy="0 0 0" xyz="0 0 -0.5"/>
<axis xyz="0 0 1"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="r_leg_link2">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="l_leg_joint1" type="revolute">
<parent link="base_link"/>
<child link="l_leg_link1"/>
<origin rpy="0 0 0" xyz="0 0.5 -0.5"/>
<axis xyz="0 1 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="l_leg_link1">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="l_leg_joint2" type="revolute">
<parent link="l_leg_link1"/>
<child link="l_leg_link2"/>
<origin rpy="0 0 0" xyz="0 0 -0.5"/>
<axis xyz="0 0 1"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="l_leg_link2">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="body_joint" type="fixed">
<parent link="base_link"/>
<child link="body_link"/>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<axis xyz="0 0 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="body_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="r_arm_joint1" type="revolute">
<parent link="body_link"/>
<child link="r_arm_link1"/>
<origin rpy="0 0 0" xyz="0 -0.5 0"/>
<axis xyz="1 0 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="r_arm_link1">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="r_arm_joint2" type="revolute">
<parent link="r_arm_link1"/>
<child link="r_arm_link2"/>
<origin rpy="0 0 0" xyz="0 0 -0.5"/>
<axis xyz="0 1 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="r_arm_link2">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="l_arm_joint1" type="revolute">
<parent link="body_link"/>
<child link="l_arm_link1"/>
<origin rpy="0 0 0" xyz="0 0.5 0"/>
<axis xyz="1 0 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="l_arm_link1">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="l_arm_joint2" type="revolute">
<parent link="l_arm_link1"/>
<child link="l_arm_link2"/>
<origin rpy="0 0 0" xyz="0 0 -0.5"/>
<axis xyz="0 1 0"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="l_arm_link2">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
<joint name="head_joint1" type="revolute">
<parent link="body_link"/>
<child link="head_link"/>
<origin rpy="0 0 0" xyz="0.5 0 0.5"/>
<axis xyz="0 0 1"/>
<limit effort="1.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="1.0"/>
<dynamics damping="1.0e-6" friction="0.8"/>
</joint>
<link name="head_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.1667e-02" ixy="0.0" ixz="0.0" iyy="0.1667e-02" iyz="0.0" izz="0.1667e-02"/>
<mass value="1.0"/>
</inertial>
</link>
</robot>
Loading

0 comments on commit 48f8b19

Please sign in to comment.