mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
1925 lines
54 KiB
C++
1925 lines
54 KiB
C++
#include "KinTree.h"
|
|
|
|
#include <iostream>
|
|
|
|
#include "RBDUtil.h"
|
|
|
|
#ifdef USE_JSON
|
|
#include "FileUtil.h"
|
|
#endif
|
|
|
|
|
|
const int cKinTree::gPosDim = 3;
|
|
const int cKinTree::gRotDim = 4;
|
|
const int cKinTree::gRootDim = gPosDim + gRotDim;
|
|
const int cKinTree::gInvalidJointID = -1;
|
|
|
|
// Json keys
|
|
const std::string gJointTypeNames[cKinTree::eJointTypeMax] =
|
|
{
|
|
"revolute",
|
|
"planar",
|
|
"prismatic",
|
|
"fixed",
|
|
"spherical",
|
|
"none"
|
|
};
|
|
|
|
const std::string gJointsKey = "Joints";
|
|
const std::string gJointDescKeys[cKinTree::eJointDescMax] =
|
|
{
|
|
"Type",
|
|
"Parent",
|
|
"AttachX",
|
|
"AttachY",
|
|
"AttachZ",
|
|
"AttachThetaX",
|
|
"AttachThetaY",
|
|
"AttachThetaZ",
|
|
"LimLow0",
|
|
"LimLow1",
|
|
"LimLow2",
|
|
"LimHigh0",
|
|
"LimHigh1",
|
|
"LimHigh2",
|
|
"TorqueLim",
|
|
"ForceLim",
|
|
"IsEndEffector",
|
|
"DiffWeight",
|
|
"Offset"
|
|
};
|
|
|
|
const std::string gBodyDefsKey = "BodyDefs";
|
|
const std::string gBodyDescKeys[cKinTree::eBodyParamMax] =
|
|
{
|
|
"Shape",
|
|
"Mass",
|
|
"ColGroup",
|
|
"EnableFallContact",
|
|
"AttachX",
|
|
"AttachY",
|
|
"AttachZ",
|
|
"AttachThetaX",
|
|
"AttachThetaY",
|
|
"AttachThetaZ",
|
|
"Param0",
|
|
"Param1",
|
|
"Param2",
|
|
"ColorR",
|
|
"ColorG",
|
|
"ColorB",
|
|
"ColorA"
|
|
};
|
|
|
|
const std::string gDrawShapeDefsKey = "DrawShapeDefs";
|
|
const std::string gDrawShapeDescKeys[cKinTree::eDrawShapeParamMax] =
|
|
{
|
|
"Shape",
|
|
"ParentJoint",
|
|
"AttachX",
|
|
"AttachY",
|
|
"AttachZ",
|
|
"AttachThetaX",
|
|
"AttachThetaY",
|
|
"AttachThetaZ",
|
|
"Param0",
|
|
"Param1",
|
|
"Param2",
|
|
"ColorR",
|
|
"ColorG",
|
|
"ColorB",
|
|
"ColorA",
|
|
"MeshID"
|
|
};
|
|
|
|
int cKinTree::GetRoot(const Eigen::MatrixXd& joint_desc)
|
|
{
|
|
// this should always be true right?
|
|
return 0;
|
|
}
|
|
|
|
void cKinTree::FindChildren(const Eigen::MatrixXd& joint_desc, int joint_id, Eigen::VectorXi& out_children)
|
|
{
|
|
const int max_size = 128;
|
|
int children_buffer[max_size];
|
|
int num_children = 0;
|
|
int num_joints = GetNumJoints(joint_desc);
|
|
|
|
for (int i = 0; i < num_joints; ++i)
|
|
{
|
|
int parent = GetParent(joint_desc, i);
|
|
if (parent == joint_id)
|
|
{
|
|
children_buffer[num_children] = i;
|
|
++num_children;
|
|
|
|
if (num_children >= max_size)
|
|
{
|
|
printf("Too many children, max = %i", max_size);
|
|
assert(false);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
out_children.resize(num_children);
|
|
for (int i = 0; i < num_children; ++i)
|
|
{
|
|
out_children[i] = children_buffer[i];
|
|
}
|
|
}
|
|
|
|
#ifdef USE_JSON
|
|
bool cKinTree::LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs)
|
|
{
|
|
bool succ = true;
|
|
std::string str;
|
|
|
|
std::ifstream f_stream(char_file.c_str());
|
|
Json::Value root;
|
|
Json::Reader reader;
|
|
succ = reader.parse(f_stream, root);
|
|
f_stream.close();
|
|
|
|
if (succ)
|
|
{
|
|
succ = false;
|
|
if (!root[gBodyDefsKey].isNull())
|
|
{
|
|
Json::Value body_defs = root.get(gBodyDefsKey, 0);
|
|
int num_bodies = body_defs.size();
|
|
|
|
succ = true;
|
|
out_body_defs.resize(num_bodies, eBodyParamMax);
|
|
for (int b = 0; b < num_bodies; ++b)
|
|
{
|
|
tBodyDef curr_def = BuildBodyDef();
|
|
Json::Value body_json = body_defs.get(b, 0);
|
|
bool succ_def = ParseBodyDef(body_json, curr_def);
|
|
|
|
if (succ)
|
|
{
|
|
out_body_defs.row(b) = curr_def;
|
|
}
|
|
else
|
|
{
|
|
succ = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!succ)
|
|
{
|
|
printf("Failed to load body definition from %s\n", char_file.c_str());
|
|
succ = false;
|
|
}
|
|
|
|
return succ;
|
|
}
|
|
|
|
bool cKinTree::ParseBodyDef(const Json::Value& root, cKinTree::tBodyDef& out_def)
|
|
{
|
|
std::string shape_str = root.get(gBodyDescKeys[eBodyParamShape], "").asString();
|
|
cShape::eShape shape;
|
|
bool succ = cShape::ParseShape(shape_str, shape);
|
|
if (succ)
|
|
{
|
|
out_def(eBodyParamShape) = static_cast<double>(static_cast<int>(shape));
|
|
}
|
|
|
|
for (int i = 0; i < eBodyParamMax; ++i)
|
|
{
|
|
const std::string& curr_key = gBodyDescKeys[i];
|
|
if (!root[curr_key].isNull()
|
|
&& root[curr_key].isNumeric())
|
|
{
|
|
Json::Value json_val = root[curr_key];
|
|
double val = json_val.asDouble();
|
|
out_def(i) = val;
|
|
}
|
|
}
|
|
|
|
return succ;
|
|
}
|
|
|
|
bool cKinTree::LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs)
|
|
{
|
|
bool succ = true;
|
|
std::string str;
|
|
|
|
std::ifstream f_stream(char_file.c_str());
|
|
Json::Value root;
|
|
Json::Reader reader;
|
|
succ = reader.parse(f_stream, root);
|
|
f_stream.close();
|
|
|
|
if (succ)
|
|
{
|
|
if (!root[gDrawShapeDefsKey].isNull())
|
|
{
|
|
Json::Value shape_defs = root.get(gDrawShapeDefsKey, 0);
|
|
int num_shapes = shape_defs.size();
|
|
|
|
succ = true;
|
|
out_draw_defs.resize(num_shapes, eDrawShapeParamMax);
|
|
for (int b = 0; b < num_shapes; ++b)
|
|
{
|
|
tDrawShapeDef curr_def = BuildDrawShapeDef();
|
|
Json::Value shape_json = shape_defs.get(b, 0);
|
|
bool succ_def = ParseDrawShapeDef(shape_json, curr_def);
|
|
|
|
if (succ)
|
|
{
|
|
out_draw_defs.row(b) = curr_def;
|
|
}
|
|
else
|
|
{
|
|
succ = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!succ)
|
|
{
|
|
printf("Failed to load draw shape definition from %s\n", char_file.c_str());
|
|
assert(false);
|
|
}
|
|
|
|
return succ;
|
|
}
|
|
|
|
bool cKinTree::ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def)
|
|
{
|
|
std::string shape_str = root.get(gDrawShapeDescKeys[eDrawShapeShape], "").asString();
|
|
cShape::eShape shape;
|
|
bool succ = cShape::ParseShape(shape_str, shape);
|
|
if (succ)
|
|
{
|
|
out_def(eDrawShapeShape) = static_cast<double>(static_cast<int>(shape));
|
|
}
|
|
|
|
for (int i = 0; i < eDrawShapeParamMax; ++i)
|
|
{
|
|
const std::string& curr_key = gDrawShapeDescKeys[i];
|
|
if (!root[curr_key].isNull()
|
|
&& root[curr_key].isNumeric())
|
|
{
|
|
Json::Value json_val = root[curr_key];
|
|
double val = json_val.asDouble();
|
|
out_def(i) = val;
|
|
}
|
|
}
|
|
|
|
return succ;
|
|
}
|
|
|
|
|
|
|
|
bool cKinTree::Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat)
|
|
{
|
|
bool succ = false;
|
|
|
|
if (!root[gJointsKey].isNull())
|
|
{
|
|
Json::Value joints = root[gJointsKey];
|
|
int num_joints = joints.size();
|
|
|
|
out_joint_mat.resize(num_joints, eJointDescMax);
|
|
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
tJointDesc curr_joint_desc = tJointDesc::Zero();
|
|
|
|
Json::Value joint_json = joints.get(j, 0);
|
|
succ = ParseJoint(joint_json, curr_joint_desc);
|
|
if (succ)
|
|
{
|
|
out_joint_mat.row(j) = curr_joint_desc;
|
|
}
|
|
else
|
|
{
|
|
printf("Failed to parse joint %i\n", j);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
const auto& curr_desc = out_joint_mat.row(j);
|
|
int parent_id = static_cast<int>(curr_desc(eJointDescParent));
|
|
if (parent_id >= j)
|
|
{
|
|
printf("Parent id must be < child id, parent id: %i, child id: %i\n", parent_id, j);
|
|
out_joint_mat.resize(0, 0);
|
|
assert(false);
|
|
|
|
return false;
|
|
}
|
|
|
|
out_joint_mat.row(j) = curr_desc;
|
|
}
|
|
|
|
PostProcessJointMat(out_joint_mat);
|
|
}
|
|
|
|
return succ;
|
|
}
|
|
bool cKinTree::ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc)
|
|
{
|
|
out_joint_desc = BuildJointDesc();
|
|
eJointType joint_type = eJointTypeNone;
|
|
const Json::Value& type_json = root[gJointDescKeys[eJointDescType]];
|
|
if (type_json.isNull())
|
|
{
|
|
printf("No joint type specified\n");
|
|
}
|
|
else
|
|
{
|
|
std::string type_str = type_json.asString();
|
|
ParseJointType(type_str, joint_type);
|
|
out_joint_desc[eJointDescType] = static_cast<double>(static_cast<int>(joint_type));
|
|
}
|
|
|
|
for (int i = 0; i < eJointDescMax; ++i)
|
|
{
|
|
if (i != eJointDescType)
|
|
{
|
|
const std::string& key = gJointDescKeys[i];
|
|
if (!root[key].isNull())
|
|
{
|
|
out_joint_desc[i] = root[key].asDouble();
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
std::string cKinTree::BuildJointMatJson(const Eigen::MatrixXd& joint_mat)
|
|
{
|
|
std::string json = "";
|
|
json += "{\n\"" + gJointsKey + "\":\n[\n";
|
|
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
tJointDesc curr_desc = joint_mat.row(j);
|
|
std::string joint_json = BuildJointJson(j, curr_desc);
|
|
|
|
if (j != 0)
|
|
{
|
|
json += ",\n";
|
|
}
|
|
|
|
json += joint_json;
|
|
}
|
|
|
|
json += "\n]\n}";
|
|
return json;
|
|
}
|
|
|
|
std::string cKinTree::BuildJointJson(int id, const tJointDesc& joint_desc)
|
|
{
|
|
std::string json = "";
|
|
json += "{\n";
|
|
json += "\"ID\": " + std::to_string(id);
|
|
for (int i = 0; i < eJointDescMax; ++i)
|
|
{
|
|
std::string param_name = gJointDescKeys[i];
|
|
if (i == eJointDescType)
|
|
{
|
|
json += ",\n";
|
|
int type_id = static_cast<int>(joint_desc[i]);
|
|
std::string type_str = gJointTypeNames[type_id];
|
|
json += "\"" + param_name + "\": \"" + type_str + "\"";
|
|
}
|
|
else if (i != eJointDescParamOffset)
|
|
{
|
|
double val = joint_desc[i];
|
|
if (!std::isfinite(val) && (i == eJointDescTorqueLim || i == eJointDescForceLim))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (i != 0)
|
|
{
|
|
json += ",\n";
|
|
}
|
|
|
|
json += "\"" + param_name + "\": " + std::to_string(val);
|
|
}
|
|
}
|
|
|
|
json += "\n}";
|
|
return json;
|
|
}
|
|
#endif //USE_JSON
|
|
|
|
tVector cKinTree::CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id)
|
|
{
|
|
assert(IsValidBody(body_defs, part_id));
|
|
tMatrix body_joint_trans = BodyJointTrans(body_defs, part_id);
|
|
tMatrix joint_to_world_trans = JointWorldTrans(joint_mat, state, part_id);
|
|
|
|
tVector attach_pt = joint_to_world_trans * body_joint_trans.col(3);
|
|
attach_pt[3] = 0;
|
|
return attach_pt;
|
|
}
|
|
|
|
tVector cKinTree::CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id)
|
|
{
|
|
tVector attach_pt = cKinTree::GetBodyAttachPt(body_defs, part_id);
|
|
return CalcWorldVel(joint_mat, state, vel, part_id, attach_pt);
|
|
}
|
|
|
|
cShape::eShape cKinTree::GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
cShape::eShape shape = static_cast<cShape::eShape>(static_cast<int>(body_defs(part_id, cKinTree::eBodyParamShape)));
|
|
return shape;
|
|
}
|
|
|
|
tVector cKinTree::GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
const cKinTree::tBodyDef& def = body_defs.row(part_id);
|
|
tVector attach_pt = tVector(def(eBodyParamAttachX), def(eBodyParamAttachY), def(eBodyParamAttachZ), 0);
|
|
return attach_pt;
|
|
}
|
|
|
|
tVector cKinTree::GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
tVector attach_theta = tVector(body_defs(part_id, eBodyParamAttachThetaX),
|
|
body_defs(part_id, eBodyParamAttachThetaY),
|
|
body_defs(part_id, eBodyParamAttachThetaZ), 0);
|
|
return attach_theta;
|
|
}
|
|
|
|
void cKinTree::GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta)
|
|
{
|
|
tVector theta = GetBodyAttachTheta(body_defs, part_id);
|
|
cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta);
|
|
}
|
|
|
|
double cKinTree::GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
double mass = body_defs(part_id, eBodyParamMass);
|
|
return mass;
|
|
}
|
|
|
|
int cKinTree::GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
int col_group = static_cast<int>(body_defs(part_id, eBodyParamColGroup));
|
|
return col_group;
|
|
}
|
|
|
|
bool cKinTree::GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
double fall_val = body_defs(part_id, eBodyParamEnableFallContact);
|
|
return fall_val != 0;
|
|
}
|
|
|
|
void cKinTree::SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs)
|
|
{
|
|
out_body_defs(part_id, eBodyParamEnableFallContact) = (enable) ? 1 : 0;
|
|
}
|
|
|
|
tVector cKinTree::GetBodySize(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
const tBodyDef& def = body_defs.row(part_id);
|
|
tVector size = tVector(def(eBodyParam0), def(eBodyParam1), def(eBodyParam2), 0);
|
|
return size;
|
|
}
|
|
|
|
tVector cKinTree::GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
const tBodyDef& def = body_defs.row(part_id);
|
|
tVector col = tVector(def(eBodyColorR), def(eBodyColorG), def(eBodyColorB), def(eBodyColorA));
|
|
return col;
|
|
}
|
|
|
|
double cKinTree::CalcTotalMass(const Eigen::MatrixXd& body_defs)
|
|
{
|
|
double total_mass = 0;
|
|
for (int i = 0; i < body_defs.rows(); ++i)
|
|
{
|
|
if (IsValidBody(body_defs, i))
|
|
{
|
|
double mass = cKinTree::GetBodyMass(body_defs, i);
|
|
total_mass += mass;
|
|
}
|
|
}
|
|
return total_mass;
|
|
}
|
|
|
|
bool cKinTree::IsValidBody(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
cShape::eShape shape = GetBodyShape(body_defs, part_id);
|
|
if (shape != cShape::eShapeNull)
|
|
{
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
tVector cKinTree::GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
cShape::eShape shape = GetBodyShape(body_defs, part_id);
|
|
tVector com = tVector::Zero();
|
|
switch (shape)
|
|
{
|
|
case cShape::eShapeBox:
|
|
case cShape::eShapeCapsule:
|
|
case cShape::eShapeSphere:
|
|
case cShape::eShapeCylinder:
|
|
break;
|
|
default:
|
|
assert(false); // unsupported
|
|
break;
|
|
}
|
|
|
|
return com;
|
|
}
|
|
|
|
int cKinTree::GetDrawShapeParentJoint(const tDrawShapeDef& shape)
|
|
{
|
|
return static_cast<int>(shape[eDrawShapeParentJoint]);
|
|
}
|
|
|
|
tVector cKinTree::GetDrawShapeAttachPt(const tDrawShapeDef& shape)
|
|
{
|
|
return tVector(shape[eDrawShapeAttachX], shape[eDrawShapeAttachY], shape[cKinTree::eDrawShapeAttachZ], 0);
|
|
}
|
|
|
|
tVector cKinTree::GetDrawShapeAttachTheta(const tDrawShapeDef& shape)
|
|
{
|
|
return tVector(shape[eDrawShapeAttachThetaX], shape[eDrawShapeAttachThetaY], shape[eDrawShapeAttachThetaZ], 0);
|
|
}
|
|
|
|
void cKinTree::GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta)
|
|
{
|
|
tVector theta = GetDrawShapeAttachTheta(shape);
|
|
cMathUtil::EulerToAxisAngle(theta, out_axis, out_theta);
|
|
}
|
|
|
|
tVector cKinTree::GetDrawShapeColor(const tDrawShapeDef& shape)
|
|
{
|
|
return tVector(shape[eDrawShapeColorR], shape[eDrawShapeColorG], shape[eDrawShapeColorB], shape[cKinTree::eDrawShapeColorA]);
|
|
}
|
|
|
|
int cKinTree::GetDrawShapeMeshID(const tDrawShapeDef& shape)
|
|
{
|
|
return static_cast<int>(shape[eDrawShapeMeshID]);
|
|
}
|
|
|
|
void cKinTree::CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta)
|
|
{
|
|
tMatrix mat = BodyWorldTrans(joint_mat, body_defs, state, part_id);
|
|
cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta);
|
|
}
|
|
|
|
|
|
|
|
bool cKinTree::HasValidRoot(const Eigen::MatrixXd& joint_desc)
|
|
{
|
|
int root = GetRoot(joint_desc);
|
|
return root != gInvalidJointID;
|
|
}
|
|
|
|
tVector cKinTree::GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
tVector pos = tVector::Zero();
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
pos.segment(0, gPosDim) = state.segment(param_offset, gPosDim);
|
|
return pos;
|
|
}
|
|
|
|
void cKinTree::SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
out_state.segment(param_offset, gPosDim) = pos.segment(0, gPosDim);
|
|
}
|
|
|
|
tQuaternion cKinTree::GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
tQuaternion rot = cMathUtil::VecToQuat(state.segment(param_offset + gPosDim, gRotDim));
|
|
return rot;
|
|
}
|
|
|
|
void cKinTree::SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
out_state.segment(param_offset + gPosDim, gRotDim) = cMathUtil::QuatToVec(rot).segment(0, gRotDim);
|
|
}
|
|
|
|
tVector cKinTree::GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
tVector pos = tVector::Zero();
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
pos.segment(0, gPosDim) = vel.segment(param_offset, gPosDim);
|
|
return pos;
|
|
}
|
|
|
|
void cKinTree::SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
out_vel.segment(param_offset, gPosDim) = vel.segment(0, gPosDim);
|
|
}
|
|
|
|
tVector cKinTree::GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
tVector ang_vel = tVector::Zero();
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
ang_vel.segment(0, gRotDim) = vel.segment(param_offset + gPosDim, gRotDim);
|
|
return ang_vel;
|
|
}
|
|
|
|
void cKinTree::SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int param_offset = GetParamOffset(joint_mat, root_id);
|
|
out_vel.segment(param_offset + gPosDim, gRotDim) = ang_vel.segment(0, gRotDim);
|
|
}
|
|
|
|
tVector cKinTree::CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tVector pos = LocalToWorldPos(joint_mat, state, joint_id, tVector::Zero());
|
|
return pos;
|
|
}
|
|
|
|
tVector cKinTree::LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt)
|
|
{
|
|
tMatrix local_to_world_trans = JointWorldTrans(joint_mat, state, parent_id);
|
|
tVector pos = attach_pt;
|
|
pos[3] = 1;
|
|
pos = local_to_world_trans * pos;
|
|
pos[3] = 0;
|
|
|
|
return pos;
|
|
}
|
|
|
|
tQuaternion cKinTree::CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix mat = JointWorldTrans(joint_mat, state, joint_id);
|
|
return cMathUtil::RotMatToQuaternion(mat);
|
|
}
|
|
|
|
void cKinTree::CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id,
|
|
tVector& out_axis, double& out_theta)
|
|
{
|
|
tMatrix mat = JointWorldTrans(joint_mat, state, joint_id);
|
|
cMathUtil::RotMatToAxisAngle(mat, out_axis, out_theta);
|
|
}
|
|
|
|
tVector cKinTree::CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id)
|
|
{
|
|
return CalcWorldVel(joint_mat, state, vel, joint_id, tVector::Zero());
|
|
}
|
|
|
|
tVector cKinTree::CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt)
|
|
{
|
|
cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id);
|
|
tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt);
|
|
cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos);
|
|
sv = cSpAlg::ApplyTransM(world_to_pt, sv);
|
|
return cSpAlg::GetV(sv);
|
|
}
|
|
|
|
tVector cKinTree::CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id)
|
|
{
|
|
return CalcWorldAngularVel(joint_mat, state, vel, joint_id, tVector::Zero());
|
|
}
|
|
|
|
tVector cKinTree::CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt)
|
|
{
|
|
cSpAlg::tSpVec sv = cRBDUtil::CalcWorldVel(joint_mat, state, vel, parent_id);
|
|
tVector pos = cKinTree::LocalToWorldPos(joint_mat, state, parent_id, attach_pt);
|
|
cSpAlg::tSpTrans world_to_pt = cSpAlg::BuildTrans(pos);
|
|
sv = cSpAlg::ApplyTransM(world_to_pt, sv);
|
|
return cSpAlg::GetOmega(sv);
|
|
}
|
|
|
|
int cKinTree::GetNumJoints(const Eigen::MatrixXd& joint_mat)
|
|
{
|
|
return static_cast<int>(joint_mat.rows());
|
|
}
|
|
|
|
int cKinTree::GetNumDof(const Eigen::MatrixXd& joint_mat)
|
|
{
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
int num_dof = cKinTree::GetParamOffset(joint_mat, num_joints - 1) + cKinTree::GetParamSize(joint_mat, num_joints - 1);
|
|
return num_dof;
|
|
}
|
|
|
|
void cKinTree::ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose)
|
|
{
|
|
int root_id = GetRoot(joint_mat);
|
|
int num_joints = cKinTree::GetNumJoints(joint_mat);
|
|
out_pose += step;
|
|
}
|
|
|
|
|
|
Eigen::VectorXi cKinTree::FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end)
|
|
{
|
|
Eigen::VectorXi chain;
|
|
|
|
const int max_length = 128;
|
|
int chain_buffer[max_length];
|
|
int buffer_idx = 0;
|
|
|
|
if (joint_beg == joint_end)
|
|
{
|
|
Eigen::VectorXi chain(1);
|
|
chain[0] = joint_beg;
|
|
}
|
|
|
|
int common_ancestor = gInvalidJointID;
|
|
int curr_id = joint_beg;
|
|
int end_len = 0;
|
|
while (curr_id != gInvalidJointID)
|
|
{
|
|
chain_buffer[buffer_idx] = curr_id;
|
|
++buffer_idx;
|
|
|
|
if (buffer_idx >= max_length)
|
|
{
|
|
printf("Exceeded maximum chain length %i\n", max_length);
|
|
assert(false);
|
|
return chain;
|
|
}
|
|
|
|
bool is_ancestor = IsAncestor(joint_mat, joint_end, curr_id, end_len);
|
|
if (is_ancestor)
|
|
{
|
|
common_ancestor = curr_id;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
curr_id = GetParent(joint_mat, curr_id);
|
|
}
|
|
}
|
|
|
|
bool found = common_ancestor != gInvalidJointID;
|
|
// tree should always connected?
|
|
assert(found);
|
|
|
|
if (found)
|
|
{
|
|
chain.resize(buffer_idx + end_len);
|
|
for (int i = 0; i < buffer_idx; ++i)
|
|
{
|
|
chain[i] = chain_buffer[i];
|
|
}
|
|
|
|
int idx = buffer_idx;
|
|
curr_id = joint_end;
|
|
while (curr_id != common_ancestor)
|
|
{
|
|
chain[idx] = curr_id;
|
|
curr_id = GetParent(joint_mat, curr_id);
|
|
++idx;
|
|
}
|
|
|
|
int num_flips = static_cast<int>(chain.size()) - buffer_idx;
|
|
chain.block(buffer_idx, 0, num_flips, 1).reverseInPlace();
|
|
}
|
|
|
|
return chain;
|
|
}
|
|
|
|
bool cKinTree::IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len)
|
|
{
|
|
int curr_id = child_joint;
|
|
out_len = 0;
|
|
while (curr_id != gInvalidJointID)
|
|
{
|
|
if (curr_id == ancestor_joint)
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
curr_id = GetParent(joint_mat, curr_id);
|
|
++out_len;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
double cKinTree::CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain)
|
|
{
|
|
double len = 0;
|
|
int num_joints = static_cast<int>(chain.size());
|
|
for (int i = 1; i < num_joints; ++i)
|
|
{
|
|
int curr_id = chain(i);
|
|
int prev_id = chain(i - 1);
|
|
|
|
if (prev_id != gInvalidJointID)
|
|
{
|
|
int prev_parent = GetParent(joint_mat, prev_id);
|
|
bool is_parent = (prev_parent == curr_id);
|
|
if (is_parent)
|
|
{
|
|
double curr_len = CalcLinkLength(joint_mat, prev_id);
|
|
len += curr_len;
|
|
}
|
|
}
|
|
|
|
if (curr_id != gInvalidJointID)
|
|
{
|
|
int curr_parent = GetParent(joint_mat, curr_id);
|
|
bool is_child = (curr_parent == prev_id);
|
|
if (is_child)
|
|
{
|
|
double curr_len = CalcLinkLength(joint_mat, curr_id);
|
|
len += curr_len;
|
|
}
|
|
}
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
void cKinTree::CalcAABB(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max)
|
|
{
|
|
out_min[0] = std::numeric_limits<double>::infinity();
|
|
out_min[1] = std::numeric_limits<double>::infinity();
|
|
out_min[2] = std::numeric_limits<double>::infinity();
|
|
|
|
out_max[0] = -std::numeric_limits<double>::infinity();
|
|
out_max[1] = -std::numeric_limits<double>::infinity();
|
|
out_max[2] = -std::numeric_limits<double>::infinity();
|
|
|
|
for (int i = 0; i < GetNumJoints(joint_desc); ++i)
|
|
{
|
|
tVector pos = CalcJointWorldPos(joint_desc, state, i);
|
|
out_min = out_min.cwiseMin(pos);
|
|
out_max = out_max.cwiseMax(pos);
|
|
}
|
|
}
|
|
|
|
int cKinTree::GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
int offset = static_cast<int>(joint_mat(joint_id, eJointDescParamOffset));
|
|
return offset;
|
|
}
|
|
|
|
int cKinTree::GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
eJointType joint_type = cKinTree::GetJointType(joint_mat, joint_id);
|
|
bool is_root = cKinTree::IsRoot(joint_mat, joint_id);
|
|
int size = (is_root) ? gRootDim : GetJointParamSize(joint_type);
|
|
return size;
|
|
}
|
|
|
|
int cKinTree::GetJointParamSize(eJointType joint_type)
|
|
{
|
|
int size = 0;
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeRevolute:
|
|
size = 1;
|
|
break;
|
|
case eJointTypePrismatic:
|
|
size = 1;
|
|
break;
|
|
case eJointTypePlanar:
|
|
size = 3;
|
|
break;
|
|
case eJointTypeFixed:
|
|
size = 0;
|
|
break;
|
|
case eJointTypeSpherical:
|
|
size = 4;
|
|
break;
|
|
default:
|
|
assert(false); // unsupported joint type
|
|
break;
|
|
}
|
|
|
|
return size;
|
|
}
|
|
|
|
void cKinTree::GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params)
|
|
{
|
|
int offset = GetParamOffset(joint_mat, j);
|
|
int dim = GetParamSize(joint_mat, j);
|
|
if (dim > 0)
|
|
{
|
|
out_params = state.block(offset, 0, dim, 1);
|
|
}
|
|
else
|
|
{
|
|
out_params = Eigen::VectorXd::Zero(1);
|
|
}
|
|
}
|
|
|
|
void cKinTree::SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state)
|
|
{
|
|
int offset = GetParamOffset(joint_mat, j);
|
|
int dim = GetParamSize(joint_mat, j);
|
|
assert(dim == params.size());
|
|
out_state.block(offset, 0, dim, 1) = params;
|
|
}
|
|
|
|
cKinTree::eJointType cKinTree::GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
eJointType type = static_cast<eJointType>(static_cast<int>(joint_mat(joint_id, cKinTree::eJointDescType)));
|
|
return type;
|
|
}
|
|
|
|
int cKinTree::GetParent(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
int parent = static_cast<int>(joint_mat(joint_id, cKinTree::eJointDescParent));
|
|
assert(parent < joint_id); // joints should always be ordered as such
|
|
// since some algorithms will assume this ordering
|
|
return parent;
|
|
}
|
|
|
|
bool cKinTree::HasParent(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
int parent = GetParent(joint_mat, joint_id);
|
|
return parent != gInvalidJointID;
|
|
}
|
|
|
|
bool cKinTree::IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
return !HasParent(joint_mat, joint_id);
|
|
}
|
|
|
|
bool cKinTree::IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
return !IsRoot(joint_mat, joint_id);
|
|
}
|
|
|
|
double cKinTree::GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
double torque_lim = joint_mat(joint_id, cKinTree::eJointDescTorqueLim);
|
|
return torque_lim;
|
|
}
|
|
|
|
double cKinTree::GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
double force_lim = joint_mat(joint_id, cKinTree::eJointDescForceLim);
|
|
return force_lim;
|
|
}
|
|
|
|
bool cKinTree::IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
double end_eff_val = joint_mat(joint_id, cKinTree::eJointDescIsEndEffector);
|
|
return end_eff_val != 0;
|
|
}
|
|
|
|
tVector cKinTree::GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
return tVector(joint_mat(joint_id, cKinTree::eJointDescLimLow0),
|
|
joint_mat(joint_id, cKinTree::eJointDescLimLow1),
|
|
joint_mat(joint_id, cKinTree::eJointDescLimLow2), 1);
|
|
}
|
|
|
|
tVector cKinTree::GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
return tVector(joint_mat(joint_id, cKinTree::eJointDescLimHigh0),
|
|
joint_mat(joint_id, cKinTree::eJointDescLimHigh1),
|
|
joint_mat(joint_id, cKinTree::eJointDescLimHigh2), 0);
|
|
}
|
|
|
|
double cKinTree::GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
return joint_mat(joint_id, cKinTree::eJointDescDiffWeight);
|
|
}
|
|
|
|
double cKinTree::CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
tVector attach_pt = GetAttachPt(joint_mat, joint_id);
|
|
bool is_root = IsRoot(joint_mat, joint_id);
|
|
double len = (is_root) ? 0 : attach_pt.norm();
|
|
return len;
|
|
}
|
|
|
|
tVector cKinTree::GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
tVector attach_pt = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachX),
|
|
joint_mat(joint_id, cKinTree::eJointDescAttachY),
|
|
joint_mat(joint_id, cKinTree::eJointDescAttachZ), 0);
|
|
return attach_pt;
|
|
}
|
|
|
|
tVector cKinTree::GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
tVector attach_theta = tVector(joint_mat(joint_id, cKinTree::eJointDescAttachThetaX),
|
|
joint_mat(joint_id, cKinTree::eJointDescAttachThetaY),
|
|
joint_mat(joint_id, cKinTree::eJointDescAttachThetaZ), 0);
|
|
return attach_theta;
|
|
}
|
|
|
|
void cKinTree::CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths)
|
|
{
|
|
int num_joints = static_cast<int>(joint_mat.rows());
|
|
out_lengths = Eigen::VectorXd::Zero(num_joints);
|
|
|
|
for (int j = num_joints - 1; j >= 0; --j)
|
|
{
|
|
int parent_id = GetParent(joint_mat, j);
|
|
if (parent_id != gInvalidJointID)
|
|
{
|
|
double curr_val = out_lengths(j);
|
|
double len = CalcLinkLength(joint_mat, j);
|
|
double& parent_val = out_lengths(parent_id);
|
|
|
|
if (parent_val < len + curr_val)
|
|
{
|
|
parent_val = len + curr_val;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void cKinTree::CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses)
|
|
{
|
|
int num_joints = static_cast<int>(joint_mat.rows());
|
|
out_masses = Eigen::VectorXd::Zero(num_joints);
|
|
|
|
for (int j = num_joints - 1; j >= 0; --j)
|
|
{
|
|
double& curr_val = out_masses(j);
|
|
double mass = GetBodyMass(body_defs, j);
|
|
curr_val += mass;
|
|
|
|
int parent_id = GetParent(joint_mat, j);
|
|
if (parent_id != gInvalidJointID)
|
|
{
|
|
double& parent_val = out_masses(parent_id);
|
|
parent_val += curr_val;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool cKinTree::ParseJointType(const std::string& type_str, eJointType& out_joint_type)
|
|
{
|
|
for (int i = 0; i < eJointTypeMax; ++i)
|
|
{
|
|
const std::string& name = gJointTypeNames[i];
|
|
if (type_str == name)
|
|
{
|
|
out_joint_type = static_cast<eJointType>(i);
|
|
return true;
|
|
}
|
|
}
|
|
printf("Unsupported joint type: %s\n", type_str.c_str());
|
|
assert(false); // unsupported joint type
|
|
return false;
|
|
}
|
|
|
|
void cKinTree::PostProcessJointMat(Eigen::MatrixXd& out_joint_mat)
|
|
{
|
|
int num_joints = GetNumJoints(out_joint_mat);
|
|
int offset = 0;
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
int curr_size = GetParamSize(out_joint_mat, j);
|
|
out_joint_mat(j, eJointDescParamOffset) = offset;
|
|
offset += curr_size;
|
|
}
|
|
int root_id = GetRoot(out_joint_mat);
|
|
|
|
out_joint_mat(root_id, eJointDescAttachX) = 0;
|
|
out_joint_mat(root_id, eJointDescAttachY) = 0;
|
|
out_joint_mat(root_id, eJointDescAttachZ) = 0;
|
|
}
|
|
|
|
tMatrix cKinTree::BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id)
|
|
{
|
|
// child to parent
|
|
tVector attach_pt = GetAttachPt(joint_mat, joint_id);
|
|
tVector attach_theta = GetAttachTheta(joint_mat, joint_id);
|
|
tMatrix mat = cMathUtil::RotateMat(attach_theta);
|
|
mat(0, 3) = attach_pt[0];
|
|
mat(1, 3) = attach_pt[1];
|
|
mat(2, 3) = attach_pt[2];
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix mat;
|
|
eJointType j_type = GetJointType(joint_mat, joint_id);
|
|
bool is_root = IsRoot(joint_mat, joint_id);
|
|
|
|
if (is_root)
|
|
{
|
|
mat = ChildParentTransRoot(joint_mat, state, joint_id);
|
|
}
|
|
else
|
|
{
|
|
switch (j_type)
|
|
{
|
|
case eJointTypeRevolute:
|
|
mat = ChildParentTransRevolute(joint_mat, state, joint_id);
|
|
break;
|
|
case eJointTypePrismatic:
|
|
mat = ChildParentTransPrismatic(joint_mat, state, joint_id);
|
|
break;
|
|
case eJointTypePlanar:
|
|
mat = ChildParentTransPlanar(joint_mat, state, joint_id);
|
|
break;
|
|
case eJointTypeFixed:
|
|
mat = ChildParentTransFixed(joint_mat, state, joint_id);
|
|
break;
|
|
case eJointTypeSpherical:
|
|
mat = ChildParentTransSpherical(joint_mat, state, joint_id);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix mat = ChildParentTrans(joint_mat, state, joint_id);
|
|
mat = cMathUtil::InvRigidMat(mat);
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix m = tMatrix::Identity();
|
|
int curr_id = joint_id;
|
|
assert(joint_id != gInvalidIdx); // invalid joint
|
|
while (curr_id != gInvalidJointID)
|
|
{
|
|
tMatrix child_parent_mat = ChildParentTrans(joint_mat, state, curr_id);
|
|
m = child_parent_mat * m;
|
|
curr_id = GetParent(joint_mat, curr_id);
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
tMatrix cKinTree::WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix m = JointWorldTrans(joint_mat, state, joint_id);
|
|
m = cMathUtil::InvRigidMat(m);
|
|
return m;
|
|
}
|
|
|
|
tMatrix cKinTree::BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id)
|
|
{
|
|
tMatrix body_trans = BodyJointTrans(body_defs, part_id);
|
|
tMatrix joint_trans = JointWorldTrans(joint_mat, state, part_id);
|
|
body_trans = joint_trans * body_trans;
|
|
return body_trans;
|
|
}
|
|
|
|
tMatrix cKinTree::BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id)
|
|
{
|
|
tVector attach_pt = GetBodyAttachPt(body_defs, part_id);
|
|
tVector euler = GetBodyAttachTheta(body_defs, part_id);
|
|
tVector com = GetBodyLocalCoM(body_defs, part_id);
|
|
|
|
tMatrix rot = cMathUtil::RotateMat(euler);
|
|
tMatrix trans = cMathUtil::TranslateMat(attach_pt + com);
|
|
tMatrix m = trans * rot;
|
|
return m;
|
|
}
|
|
|
|
cKinTree::tJointDesc cKinTree::BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt)
|
|
{
|
|
tJointDesc desc = BuildJointDesc();
|
|
desc(eJointDescType) = static_cast<double>(joint_type);
|
|
desc(eJointDescParent) = parent_id;
|
|
desc(eJointDescAttachX) = attach_pt[0];
|
|
desc(eJointDescAttachY) = attach_pt[1];
|
|
desc(eJointDescAttachZ) = attach_pt[2];
|
|
|
|
return desc;
|
|
}
|
|
|
|
cKinTree::tJointDesc cKinTree::BuildJointDesc()
|
|
{
|
|
tJointDesc desc;
|
|
desc(eJointDescType) = static_cast<double>(eJointTypeRevolute);
|
|
desc(eJointDescParent) = gInvalidIdx;
|
|
desc(eJointDescAttachX) = 0;
|
|
desc(eJointDescAttachY) = 0;
|
|
desc(eJointDescAttachZ) = 0;
|
|
desc(eJointDescAttachThetaX) = 0;
|
|
desc(eJointDescAttachThetaY) = 0;
|
|
desc(eJointDescAttachThetaZ) = 0;
|
|
desc(eJointDescLimLow0) = 1;
|
|
desc(eJointDescLimLow1) = 1;
|
|
desc(eJointDescLimLow2) = 1;
|
|
desc(eJointDescLimHigh0) = 0;
|
|
desc(eJointDescLimHigh1) = 0;
|
|
desc(eJointDescLimHigh2) = 0;
|
|
desc(eJointDescIsEndEffector) = 0;
|
|
desc(eJointDescTorqueLim) = std::numeric_limits<double>::infinity();
|
|
desc(eJointDescForceLim) = std::numeric_limits<double>::infinity();
|
|
desc(eJointDescDiffWeight) = 1;
|
|
desc(eJointDescParamOffset) = 0;
|
|
|
|
return desc;
|
|
}
|
|
|
|
cKinTree::tBodyDef cKinTree::BuildBodyDef()
|
|
{
|
|
tBodyDef def;
|
|
def(eBodyParamShape) = static_cast<double>(cShape::eShapeNull);
|
|
def(eBodyParamMass) = 0;
|
|
def(eBodyParamColGroup) = gInvalidIdx;
|
|
def(eBodyParamEnableFallContact) = 0;
|
|
def(eBodyParamAttachX) = 0;
|
|
def(eBodyParamAttachY) = 0;
|
|
def(eBodyParamAttachZ) = 0;
|
|
def(eBodyParamAttachThetaX) = 0;
|
|
def(eBodyParamAttachThetaY) = 0;
|
|
def(eBodyParamAttachThetaZ) = 0;
|
|
def(eBodyParam0) = 0;
|
|
def(eBodyParam1) = 0;
|
|
def(eBodyParam2) = 0;
|
|
def(eBodyColorR) = 0;
|
|
def(eBodyColorG) = 0;
|
|
def(eBodyColorB) = 0;
|
|
def(eBodyColorA) = 1;
|
|
return def;
|
|
}
|
|
|
|
cKinTree::tDrawShapeDef cKinTree::BuildDrawShapeDef()
|
|
{
|
|
tDrawShapeDef def;
|
|
def(eDrawShapeShape) = static_cast<double>(cShape::eShapeNull);
|
|
def(eDrawShapeParentJoint) = gInvalidIdx;
|
|
def(eDrawShapeAttachX) = 0;
|
|
def(eDrawShapeAttachY) = 0;
|
|
def(eDrawShapeAttachZ) = 0;
|
|
def(eDrawShapeAttachThetaX) = 0;
|
|
def(eDrawShapeAttachThetaY) = 0;
|
|
def(eDrawShapeAttachThetaZ) = 0;
|
|
def(eDrawShapeParam0) = 0;
|
|
def(eDrawShapeParam1) = 0;
|
|
def(eDrawShapeParam2) = 0;
|
|
def(eDrawShapeColorR) = 0;
|
|
def(eDrawShapeColorG) = 0;
|
|
def(eDrawShapeColorB) = 0;
|
|
def(eDrawShapeColorA) = 1;
|
|
def(eDrawShapeMeshID) = gInvalidIdx;
|
|
return def;
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
|
|
{
|
|
int num_dof = GetNumDof(joint_mat);
|
|
out_pose = Eigen::VectorXd::Zero(num_dof);
|
|
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
|
|
int root_id = GetRoot(joint_mat);
|
|
Eigen::VectorXd root_pose;
|
|
BuildDefaultPoseRoot(joint_mat, root_pose);
|
|
SetJointParams(joint_mat, root_id, root_pose, out_pose);
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
Eigen::VectorXd joint_pose;
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeRevolute:
|
|
BuildDefaultPoseRevolute(joint_pose);
|
|
break;
|
|
case eJointTypePrismatic:
|
|
BuildDefaultPosePrismatic(joint_pose);
|
|
break;
|
|
case eJointTypePlanar:
|
|
BuildDefaultPosePlanar(joint_pose);
|
|
break;
|
|
case eJointTypeFixed:
|
|
BuildDefaultPoseFixed(joint_pose);
|
|
break;
|
|
case eJointTypeSpherical:
|
|
BuildDefaultPoseSpherical(joint_pose);
|
|
break;
|
|
default:
|
|
assert(false); // unsupported joint type
|
|
break;
|
|
}
|
|
|
|
SetJointParams(joint_mat, j, joint_pose, out_pose);
|
|
}
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel)
|
|
{
|
|
int num_dof = GetNumDof(joint_mat);
|
|
out_vel = Eigen::VectorXd::Zero(num_dof);
|
|
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
|
|
int root_id = GetRoot(joint_mat);
|
|
Eigen::VectorXd root_pose;
|
|
BuildDefaultVelRoot(joint_mat, root_pose);
|
|
SetJointParams(joint_mat, root_id, root_pose, out_vel);
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
Eigen::VectorXd joint_pose;
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeRevolute:
|
|
BuildDefaultVelRevolute(joint_pose);
|
|
break;
|
|
case eJointTypePrismatic:
|
|
BuildDefaultVelPrismatic(joint_pose);
|
|
break;
|
|
case eJointTypePlanar:
|
|
BuildDefaultVelPlanar(joint_pose);
|
|
break;
|
|
case eJointTypeFixed:
|
|
BuildDefaultVelFixed(joint_pose);
|
|
break;
|
|
case eJointTypeSpherical:
|
|
BuildDefaultVelSpherical(joint_pose);
|
|
break;
|
|
default:
|
|
assert(false); // unsupported joint type
|
|
break;
|
|
}
|
|
|
|
SetJointParams(joint_mat, j, joint_pose, out_vel);
|
|
}
|
|
}
|
|
|
|
void cKinTree::CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff)
|
|
{
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
assert(out_diff.size() == pose0.size());
|
|
assert(pose1.size() == pose0.size());
|
|
|
|
out_diff.resize(pose1.size());
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
Eigen::VectorXd curr_diff;
|
|
CalcJointPoseDiff(joint_mat, j, pose0, pose1, curr_diff);
|
|
|
|
int param_offset = GetParamOffset(joint_mat, j);
|
|
int param_size = GetParamSize(joint_mat, j);
|
|
out_diff.segment(param_offset, param_size) = curr_diff;
|
|
}
|
|
}
|
|
|
|
tVector cKinTree::CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tVector root_pos0 = GetRootPos(joint_mat, pose0);
|
|
tVector root_pos1 = GetRootPos(joint_mat, pose1);
|
|
return root_pos1 - root_pos0;
|
|
}
|
|
|
|
tQuaternion cKinTree::CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
|
|
tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
|
|
return cMathUtil::QuatDiff(root_rot0, root_rot1);
|
|
}
|
|
|
|
double cKinTree::CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
bool is_root = IsRoot(joint_mat, joint_id);
|
|
double err = 0;
|
|
if (is_root)
|
|
{
|
|
double root_pos_err = CalcRootPosErr(joint_mat, pose0, pose1);
|
|
double root_rot_err = CalcRootRotErr(joint_mat, pose0, pose1);
|
|
err = root_pos_err + root_rot_err;
|
|
}
|
|
else
|
|
{
|
|
Eigen::VectorXd diff;
|
|
CalcJointPoseDiff(joint_mat, joint_id, pose0, pose1, diff);
|
|
|
|
eJointType joint_type = GetJointType(joint_mat, joint_id);
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeSpherical:
|
|
{
|
|
tQuaternion dq = cMathUtil::VecToQuat(diff);
|
|
double theta = cMathUtil::QuatTheta(dq);
|
|
err = theta * theta;
|
|
break;
|
|
}
|
|
default:
|
|
err = diff.squaredNorm();
|
|
break;
|
|
}
|
|
}
|
|
return err;
|
|
}
|
|
|
|
double cKinTree::CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tVector diff = CalcRootPosDiff(joint_mat, pose0, pose1);
|
|
return diff.squaredNorm();
|
|
}
|
|
|
|
double cKinTree::CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tQuaternion diff = CalcRootRotDiff(joint_mat, pose0, pose1);
|
|
double theta = cMathUtil::QuatTheta(diff);
|
|
return theta * theta;
|
|
}
|
|
|
|
|
|
void cKinTree::CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff)
|
|
{
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
assert(vel0.size() == vel1.size());
|
|
|
|
out_diff.resize(vel0.size());
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
Eigen::VectorXd curr_diff;
|
|
CalcJointVelDiff(joint_mat, j, vel0, vel1, curr_diff);
|
|
|
|
int param_offset = GetParamOffset(joint_mat, j);
|
|
int param_size = GetParamSize(joint_mat, j);
|
|
out_diff.segment(param_offset, param_size) = curr_diff;
|
|
}
|
|
}
|
|
|
|
tVector cKinTree::CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tVector root_vel0 = GetRootVel(joint_mat, pose0);
|
|
tVector root_vel1 = GetRootVel(joint_mat, pose1);
|
|
return root_vel1 - root_vel0;
|
|
}
|
|
|
|
tVector cKinTree::CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1)
|
|
{
|
|
tVector root_ang_vel0 = GetRootAngVel(joint_mat, pose0);
|
|
tVector root_ang_vel1 = GetRootAngVel(joint_mat, pose1);
|
|
return root_ang_vel1 - root_ang_vel0;
|
|
}
|
|
|
|
double cKinTree::CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
|
|
{
|
|
bool is_root = IsRoot(joint_mat, joint_id);
|
|
double err = 0;
|
|
if (is_root)
|
|
{
|
|
double root_vel_err = CalcRootVelErr(joint_mat, vel0, vel1);
|
|
double root_ang_vel_err = CalcRootAngVelErr(joint_mat, vel0, vel1);
|
|
err = root_vel_err + root_ang_vel_err;
|
|
}
|
|
else
|
|
{
|
|
Eigen::VectorXd diff;
|
|
CalcJointVelDiff(joint_mat, joint_id, vel0, vel1, diff);
|
|
err = diff.squaredNorm();
|
|
}
|
|
return err;
|
|
}
|
|
|
|
double cKinTree::CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
|
|
{
|
|
tVector diff = CalcRootVelDiff(joint_mat, vel0, vel1);
|
|
return diff.squaredNorm();
|
|
}
|
|
|
|
double cKinTree::CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1)
|
|
{
|
|
tVector diff = CalcRootAngVelDiff(joint_mat, vel0, vel1);
|
|
return diff.squaredNorm();
|
|
}
|
|
|
|
|
|
void cKinTree::CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff)
|
|
{
|
|
int param_offset = GetParamOffset(joint_mat, joint_id);
|
|
int param_size = GetParamSize(joint_mat, joint_id);
|
|
bool is_root = cKinTree::IsRoot(joint_mat, joint_id);
|
|
|
|
if (is_root)
|
|
{
|
|
tVector root_pos_diff = CalcRootPosDiff(joint_mat, pose0, pose1);
|
|
tQuaternion root_rot_diff = CalcRootRotDiff(joint_mat, pose0, pose1);
|
|
out_diff.resize(gRootDim);
|
|
out_diff.segment(0, gPosDim) = root_pos_diff.segment(0, gPosDim);
|
|
out_diff.segment(gPosDim, gRotDim) = cMathUtil::QuatToVec(root_rot_diff);
|
|
}
|
|
else
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, joint_id);
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeSpherical:
|
|
{
|
|
tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size));
|
|
tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size));
|
|
tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1);
|
|
out_diff = cMathUtil::QuatToVec(q_diff);
|
|
break;
|
|
}
|
|
default:
|
|
out_diff = pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void cKinTree::CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff)
|
|
{
|
|
int param_offset = GetParamOffset(joint_mat, joint_id);
|
|
int param_size = GetParamSize(joint_mat, joint_id);
|
|
out_diff = vel1.segment(param_offset, param_size) - vel0.segment(param_offset, param_size);
|
|
}
|
|
|
|
void cKinTree::CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel)
|
|
{
|
|
assert(pose0.size() == pose1.size());
|
|
out_vel.resize(pose0.size());
|
|
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
tVector root_pos0 = GetRootPos(joint_mat, pose0);
|
|
tVector root_pos1 = GetRootPos(joint_mat, pose1);
|
|
tVector root_vel = (root_pos1 - root_pos0) / dt;
|
|
|
|
tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
|
|
tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
|
|
tVector root_rot_vel = cMathUtil::CalcQuaternionVelRel(root_rot0, root_rot1, dt);
|
|
|
|
cKinTree::SetRootVel(joint_mat, root_vel, out_vel);
|
|
cKinTree::SetRootAngVel(joint_mat, root_rot_vel, out_vel);
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
int param_offset = GetParamOffset(joint_mat, j);
|
|
int param_size = GetParamSize(joint_mat, j);
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
|
|
switch (joint_type)
|
|
{
|
|
case eJointTypeSpherical:
|
|
{
|
|
tQuaternion q0 = cMathUtil::VecToQuat(pose0.segment(param_offset, param_size));
|
|
tQuaternion q1 = cMathUtil::VecToQuat(pose1.segment(param_offset, param_size));
|
|
tVector rot_vel = cMathUtil::CalcQuaternionVelRel(q0, q1, dt);
|
|
out_vel.segment(param_offset, param_size) = rot_vel.segment(0, param_size);
|
|
break;
|
|
}
|
|
default:
|
|
out_vel.segment(param_offset, param_size) = (pose1.segment(param_offset, param_size) - pose0.segment(param_offset, param_size)) / dt;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void cKinTree::PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
|
|
{
|
|
// mostly to normalize quaternions
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
int root_id = GetRoot(joint_mat);
|
|
int root_offset = GetParamOffset(joint_mat, root_id);
|
|
out_pose.segment(root_offset + gPosDim, gRotDim).normalize();
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
if (joint_type == eJointTypeSpherical)
|
|
{
|
|
int offset = GetParamOffset(joint_mat, j);
|
|
out_pose.segment(offset, gRotDim).normalize();
|
|
}
|
|
}
|
|
}
|
|
|
|
void cKinTree::LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose)
|
|
{
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
int root_id = GetRoot(joint_mat);
|
|
int root_offset = GetParamOffset(joint_mat, root_id);
|
|
|
|
out_pose.resize(pose0.size());
|
|
assert(pose0.size() == pose1.size());
|
|
|
|
tVector root_pos0 = GetRootPos(joint_mat, pose0);
|
|
tVector root_pos1 = GetRootPos(joint_mat, pose1);
|
|
tVector root_pos_lerp = (1 - lerp) * root_pos0 + lerp * root_pos1;
|
|
|
|
tQuaternion root_rot0 = GetRootRot(joint_mat, pose0);
|
|
tQuaternion root_rot1 = GetRootRot(joint_mat, pose1);
|
|
assert(std::abs(root_rot0.norm() - 1) < 0.000001);
|
|
assert(std::abs(root_rot1.norm() - 1) < 0.000001);
|
|
|
|
tQuaternion root_rot_lerp = root_rot0.slerp(lerp, root_rot1);
|
|
root_rot_lerp.normalize();
|
|
|
|
cKinTree::SetRootPos(joint_mat, root_pos_lerp, out_pose);
|
|
cKinTree::SetRootRot(joint_mat, root_rot_lerp, out_pose);
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
int offset = GetParamOffset(joint_mat, j);
|
|
int size = GetParamSize(joint_mat, j);
|
|
if (joint_type == eJointTypeSpherical)
|
|
{
|
|
tQuaternion rot0 = cMathUtil::VecToQuat(pose0.segment(offset, size));
|
|
tQuaternion rot1 = cMathUtil::VecToQuat(pose1.segment(offset, size));
|
|
assert(std::abs(rot0.norm() - 1) < 0.000001);
|
|
assert(std::abs(rot1.norm() - 1) < 0.000001);
|
|
tQuaternion rot_lerp = rot0.slerp(lerp, rot1);
|
|
out_pose.segment(offset, size) = cMathUtil::QuatToVec(rot_lerp).segment(0, size);
|
|
}
|
|
else
|
|
{
|
|
out_pose.segment(offset, size) = (1 - lerp) * pose0.segment(offset, size) + lerp * pose1.segment(offset, size);
|
|
}
|
|
}
|
|
}
|
|
|
|
void cKinTree::VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff)
|
|
{
|
|
out_pose_diff = vel;
|
|
|
|
int num_joints = GetNumJoints(joint_mat);
|
|
int root_id = GetRoot(joint_mat);
|
|
int root_offset = GetParamOffset(joint_mat, root_id);
|
|
|
|
tVector root_rot_vel = GetRootAngVel(joint_mat, vel);
|
|
root_rot_vel[3] = 0;
|
|
tQuaternion root_rot = GetRootRot(joint_mat, pose);
|
|
tMatrix root_diff_mat = cMathUtil::BuildQuaternionDiffMat(root_rot);
|
|
tVector root_diff = root_diff_mat * root_rot_vel;
|
|
tQuaternion root_quat = cMathUtil::VecToQuat(root_diff);
|
|
|
|
cKinTree::SetRootRot(joint_mat, root_quat, out_pose_diff);
|
|
|
|
for (int j = 1; j < num_joints; ++j)
|
|
{
|
|
eJointType joint_type = GetJointType(joint_mat, j);
|
|
int offset = GetParamOffset(joint_mat, j);
|
|
int size = GetParamSize(joint_mat, j);
|
|
if (joint_type == eJointTypeSpherical)
|
|
{
|
|
tQuaternion rot = cMathUtil::VecToQuat(pose.segment(offset, size));
|
|
tVector rot_vel = vel.segment(offset, size);
|
|
rot_vel[3] = 0;
|
|
tMatrix diff_mat = cMathUtil::BuildQuaternionDiffMat(rot);
|
|
tVector q_diff = diff_mat * rot_vel;
|
|
|
|
out_pose_diff.segment(offset, size) = q_diff;
|
|
}
|
|
}
|
|
}
|
|
|
|
double cKinTree::CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
|
|
{
|
|
// heading is the direction of the root in the xz plane
|
|
tVector ref_dir = tVector(1, 0, 0, 0);
|
|
tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, pose);
|
|
printf("root_rot=%f,%f,%f,%f\n", root_rot.x(), root_rot.y(), root_rot.z(), root_rot.w());
|
|
tVector rot_dir = cMathUtil::QuatRotVec(root_rot, ref_dir);
|
|
double heading = std::atan2(-rot_dir[2], rot_dir[0]);
|
|
return heading;
|
|
}
|
|
|
|
tQuaternion cKinTree::CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
|
|
{
|
|
double heading = CalcHeading(joint_mat, pose);
|
|
return cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), heading);
|
|
}
|
|
|
|
tMatrix cKinTree::BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
|
|
{
|
|
double heading = CalcHeading(joint_mat, pose);
|
|
tVector axis = tVector(0, 1, 0, 0);
|
|
printf("heading=%f\n", heading);
|
|
tMatrix mat = cMathUtil::RotateMat(axis, -heading);
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose)
|
|
{
|
|
// world to origin transform
|
|
// origin is the point right under the root of the character on the xz plane with x-axis
|
|
// aligned along the character's heading
|
|
tVector origin = GetRootPos(joint_mat, pose);
|
|
printf("cKinTree::BuildOriginTrans: origin=%f,%f,%f\n", origin[0], origin[1], origin[2]);
|
|
origin[1] = 0;
|
|
tMatrix rot_mat = BuildHeadingTrans(joint_mat, pose);
|
|
printf("headingMat = \n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f",
|
|
rot_mat(0, 0), rot_mat(0, 1), rot_mat(0, 2), rot_mat(0, 3),
|
|
rot_mat(1, 0), rot_mat(1, 1), rot_mat(1, 2), rot_mat(1, 3),
|
|
rot_mat(2, 0), rot_mat(2, 1), rot_mat(2, 2), rot_mat(2, 3),
|
|
rot_mat(3, 0), rot_mat(3, 1), rot_mat(3, 2), rot_mat(3, 3));
|
|
|
|
tMatrix trans_mat = cMathUtil::TranslateMat(-origin);
|
|
tMatrix mat = rot_mat * trans_mat;
|
|
printf("mat = \n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f",
|
|
mat(0,0), mat(0, 1), mat(0, 2), mat(0, 3),
|
|
mat(1, 0), mat(1, 1), mat(1, 2), mat(1, 3),
|
|
mat(2, 0), mat(2, 1), mat(2, 2), mat(2, 3),
|
|
mat(3, 0), mat(3, 1), mat(3, 2), mat(3, 3));
|
|
return mat;
|
|
}
|
|
|
|
void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
|
|
{
|
|
Eigen::VectorXd vel= Eigen::VectorXd::Zero(0);
|
|
NormalizePoseHeading(joint_mat, out_pose, vel);
|
|
}
|
|
|
|
void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel)
|
|
{
|
|
tVector root_pos = GetRootPos(joint_mat, out_pose);
|
|
tQuaternion root_rot = GetRootRot(joint_mat, out_pose);
|
|
|
|
root_pos[0] = 0;
|
|
root_pos[2] = 0;
|
|
|
|
double heading = CalcHeading(joint_mat, out_pose);
|
|
tQuaternion heading_q = cMathUtil::AxisAngleToQuaternion(tVector(0, 1, 0, 0), -heading);
|
|
root_rot = heading_q * root_rot;
|
|
|
|
SetRootPos(joint_mat, root_pos, out_pose);
|
|
SetRootRot(joint_mat, root_rot, out_pose);
|
|
|
|
if (out_vel.size() > 0)
|
|
{
|
|
tVector root_vel = GetRootVel(joint_mat, out_vel);
|
|
tVector root_ang_vel = GetRootAngVel(joint_mat, out_vel);
|
|
root_vel = cMathUtil::QuatRotVec(heading_q, root_vel);
|
|
root_ang_vel = cMathUtil::QuatRotVec(heading_q, root_ang_vel);
|
|
|
|
SetRootVel(joint_mat, root_vel, out_vel);
|
|
SetRootAngVel(joint_mat, root_ang_vel, out_vel);
|
|
}
|
|
}
|
|
|
|
|
|
tMatrix cKinTree::ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tVector offset = GetRootPos(joint_mat, state);
|
|
tQuaternion rot = GetRootRot(joint_mat, state);
|
|
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix R = cMathUtil::RotateMat(rot);
|
|
tMatrix T = cMathUtil::TranslateMat(offset);
|
|
|
|
tMatrix mat = A * T * R;
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
|
|
double theta = state(param_offset);
|
|
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta);
|
|
|
|
tMatrix mat = A * R;
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
|
|
double theta = state(param_offset + 2);
|
|
tVector offset = tVector::Zero();
|
|
offset[0] = state(param_offset);
|
|
offset[1] = state(param_offset + 1);
|
|
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix R = cMathUtil::RotateMat(tVector(0, 0, 1, 0), theta);
|
|
tMatrix T = cMathUtil::TranslateMat(offset);
|
|
|
|
tMatrix mat = A * R * T;
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix mat = A;
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
|
|
tVector offset = tVector::Zero();
|
|
offset[0] = state(param_offset);
|
|
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix T = cMathUtil::TranslateMat(offset);
|
|
|
|
tMatrix mat = A * T;
|
|
return mat;
|
|
}
|
|
|
|
tMatrix cKinTree::ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id)
|
|
{
|
|
int param_offset = cKinTree::GetParamOffset(joint_mat, joint_id);
|
|
int param_size = cKinTree::GetParamSize(joint_mat, joint_id);
|
|
tQuaternion q = cMathUtil::VecToQuat(state.segment(param_offset, param_size));
|
|
|
|
tMatrix A = BuildAttachTrans(joint_mat, joint_id);
|
|
tMatrix R = cMathUtil::RotateMat(q);
|
|
|
|
tMatrix mat = A * R;
|
|
return mat;
|
|
}
|
|
|
|
|
|
|
|
void cKinTree::BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = gRootDim;
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
out_pose(gPosDim) = 1;
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeRevolute);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypePrismatic);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPosePlanar(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypePlanar);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPoseFixed(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeFixed);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeSpherical);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
out_pose(0) = 1;
|
|
}
|
|
|
|
|
|
void cKinTree::BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = gRootDim;
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVelRevolute(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeRevolute);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypePrismatic);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVelPlanar(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypePlanar);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVelFixed(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeFixed);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|
|
|
|
void cKinTree::BuildDefaultVelSpherical(Eigen::VectorXd& out_pose)
|
|
{
|
|
int dim = GetJointParamSize(eJointTypeSpherical);
|
|
out_pose = Eigen::VectorXd::Zero(dim);
|
|
}
|