ae8e83988b
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py) Fix related to TinyRenderer object transforms not updating when using collision filtering
1983 lines
55 KiB
C++
1983 lines
55 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);
|
|
}
|
|
}
|
|
|
|
void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector<int> mirror_joints0, const std::vector<int> mirror_joints1, Eigen::VectorXd& out_pose)
|
|
{
|
|
// mirrors along xy plane
|
|
assert(mirror_joints0.size() == mirror_joints1.size());
|
|
int num_joints = cKinTree::GetNumJoints(joint_mat);
|
|
|
|
tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, out_pose);
|
|
tVector root_pos = cKinTree::GetRootPos(joint_mat, out_pose);
|
|
root_pos[2] *= -1;
|
|
root_rot = cMathUtil::MirrorQuaternion(root_rot, cMathUtil::eAxisZ);
|
|
|
|
cKinTree::SetRootRot(joint_mat, root_rot, out_pose);
|
|
cKinTree::SetRootPos(joint_mat, root_pos, out_pose);
|
|
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
cKinTree::eJointType joint_type = cKinTree::GetJointType(joint_mat, j);
|
|
bool is_root = cKinTree::IsRoot(joint_mat, j);
|
|
if (!is_root)
|
|
{
|
|
int param_offset = cKinTree::GetParamOffset(joint_mat, j);
|
|
int param_size = cKinTree::GetParamSize(joint_mat, j);
|
|
auto joint_params = out_pose.segment(param_offset, param_size);
|
|
|
|
if (joint_type == cKinTree::eJointTypeRevolute
|
|
|| joint_type == cKinTree::eJointTypeFixed)
|
|
{
|
|
}
|
|
else if (joint_type == cKinTree::eJointTypeSpherical)
|
|
{
|
|
tQuaternion quat = cMathUtil::VecToQuat(joint_params);
|
|
quat = cMathUtil::MirrorQuaternion(quat, cMathUtil::eAxisZ);
|
|
joint_params = cMathUtil::QuatToVec(quat);
|
|
}
|
|
else
|
|
{
|
|
assert(false); // unsupported joint type
|
|
}
|
|
}
|
|
}
|
|
|
|
for (size_t i = 0; i < mirror_joints0.size(); ++i)
|
|
{
|
|
int id0 = mirror_joints0[i];
|
|
int id1 = mirror_joints1[i];
|
|
|
|
int offset0 = cKinTree::GetParamOffset(joint_mat, id0);
|
|
int size0 = cKinTree::GetParamSize(joint_mat, id0);
|
|
int offset1 = cKinTree::GetParamOffset(joint_mat, id1);
|
|
int size1 = cKinTree::GetParamSize(joint_mat, id1);
|
|
|
|
Eigen::VectorXd params0 = out_pose.segment(offset0, size0);
|
|
Eigen::VectorXd params1 = out_pose.segment(offset1, size1);
|
|
out_pose.segment(offset0, size0) = params1;
|
|
out_pose.segment(offset1, size1) = params0;
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
}
|