Program Listing for File link.hpp
↰ Return to documentation for file (/tmp/ws/src/rt_manipulators_cpp/rt_manipulators_lib/include/link.hpp
)
// Copyright 2021 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_LINK_HPP_
#define RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_
#include <eigen3/Eigen/Dense>
#include <algorithm>
#include <string>
namespace manipulators_link {
// Linkクラスの参考情報:
// 梶田秀司 編著, ヒューマノイドロボット 改訂2版, オーム社, 2020
class Link{
public:
Link():
name("ダミーリンク"), sibling(0), child(0), parent(0), q(0), dq(0), ddq(0), m(0), dxl_id(0),
min_q(0), max_q(0) {
// 0ベクトル、単位行列で初期化
p.setZero();
R.setIdentity();
v.setZero();
w.setZero();
a.setZero();
b.setZero();
c.setZero();
I.setIdentity();
}
std::string name; // リンク名
int sibling; // 姉妹兄弟リンクID
int child; // 子リンクID
int parent; // 親リンクID
Eigen::Vector3d p; // ワールド座標系での位置
Eigen::Matrix3d R; // ワールド座標系での姿勢
Eigen::Vector3d v; // ワールド座標系での速度
Eigen::Vector3d w; // ワールド座標系での角速度ベクトル
double q; // 関節位置
double dq; // 関節速度
double ddq; // 関節加速度
Eigen::Vector3d a; // 親リンクに対する関節軸ベクトル
Eigen::Vector3d b; // 親リンクに対する相対位置
double m; // 質量
Eigen::Vector3d c; // 自リンクに対する重心位置
Eigen::Matrix3d I; // 自リンクに対する慣性テンソル
int dxl_id; // 対応するDynamixelのID
double min_q; // 関節位置の下限値
double max_q; // 関節位置の上限値
void set_q_within_limit(const double & desired_q) { q = std::clamp(desired_q, min_q, max_q); }
};
} // namespace manipulators_link
#endif // RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_