bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
{
bool solved;
if(ikTypeId == 0 || isBestEffortIKMode){
solved = JointPath::calcInverseKinematics(end_p, end_R);
} else {
std::vector<double> qorg(numJoints());
for(int i=0; i < numJoints(); ++i){
qorg[i] = joint(i)->q;
}
const Link* targetLink = endLink();
const Link* baseLink_ = baseLink();
Vector3 p_relative;
Matrix33 R_relative;
if(!isCustomizedIkPathReversed){
p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p);
R_relative.noalias() = baseLink_->R.transpose() * end_R;
} else {
p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p);
R_relative.noalias() = end_R.transpose() * baseLink_->R;
}
solved = body->customizerInterface->
calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative);
if(solved){
calcForwardKinematics();
Vector3 dp(end_p - targetLink->p);
Vector3 omega(omegaFromRot(targetLink->R.transpose() * end_R));
double errsqr = dp.dot(dp) + omega.dot(omega);
if(errsqr < maxIKErrorSqr){
solved = true;
} else {
solved = false;
for(int i=0; i < numJoints(); ++i){
joint(i)->q = qorg[i];
}
calcForwardKinematics();
}
}
}
return solved;
}
int main(int arg, char* argc[]) {
DetermineCameraPositionParser parser(helpMsg);
try{
parser.parse(arg,argc);
MarkerType markerType;
vector<Point2d> markers;
string dataFolder = parser.get<string>("dataFolder");
int camID = parser.get<int>("camID");
int camW = parser.get<int>("camWidth");
int camH = parser.get<int>("camHeight");
string markerName = dataFolder + "/markers/" + parser.get<string>("markerName") + ".txt";
markerType = MarkerType(markerName);
string distanceFile = dataFolder + "/distances/" + parser.get<string>("distanceMatrix") + ".txt";
MatrixDD distances = loadDistanceMatrix(distanceFile);
Mat src;
string cameraFileName = dataFolder + "/camera/" + parser.get<string>("cameraName") + ".txt";
Camera cam = loadCameraFromFile( cameraFileName );
VideoCapture cap(camID);
cap.set(CV_CAP_PROP_FRAME_WIDTH,cam.getResWidth());
cap.set(CV_CAP_PROP_FRAME_HEIGHT,cam.getResHeight());
if (!cap.isOpened())
return false;
Matrix33 planeR = Matrix33::Identity();
Vector3 planeT;
while (1) {
cap >> src;
// Track markers
trackMarker(src, markerType, markers);
// sort markers in circular patern
reorganizePoints(markers);
// print markers in picture
for (size_t i = 0; i < markers.size(); i++) {
circle(src, markers[i], 1, Scalar(255, 0, 0), 2);
stringstream ss;
ss << i;
putText(src, ss.str(), markers[i], FONT_HERSHEY_PLAIN, 1.5,
Scalar(255, 0, 0));
}
if (markers.size() == distances.rows() ) {
vector<Vector3> outPoints;
double optiError;
vector<Real> x(distances.rows(),2000);
optiError = findPointPositions(distances, markers, cam, outPoints, x);
findPlaneCoordinateFrame( cam, outPoints, planeR, planeT );
cout << "Camera is: " << (planeR.transpose()*(cam.getT() - planeT))[2] << "mm above ground." << endl;
cout << "Marker distances are: ";
for(Real y : x){
cout << y << " ";
}
cout << endl << endl;
}else{
cout << "Detected " << markers.size() << " ground markers but there should be " << distances.rows() << " markers in the picture!" << endl;
}
imshow("determineCameraPosition", src);
if (waitKey(1) >= 0)
break;
}
}catch( std::exception const & exc ){
cerr << exc.what() << endl;
return 0;
}
return 0;
}
void BodyInfo_impl::setSegmentParameters(int linkInfoIndex, JointNodeSetPtr jointNodeSet)
{
LinkInfo& linkInfo = links_[linkInfoIndex];
vector<VrmlProtoInstancePtr>& segmentNodes = jointNodeSet->segmentNodes;
int numSegment = segmentNodes.size();
linkInfo.mass = 0.0;
for( int i = 0 ; i < 3 ; ++i ) {
linkInfo.centerOfMass[i] = 0.0;
for( int j = 0 ; j < 3 ; ++j ) {
linkInfo.inertia[i*3 + j] = 0.0;
}
}
// Mass = Σmass //
// C = (Σmass * T * c) / Mass //
// I = Σ(R * I * Rt + G) //
// R = Tの回転行列 //
// G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y //
// (x, y, z ) = T * c - C //
std::vector<Vector4, Eigen::aligned_allocator<Vector4> > centerOfMassArray;
std::vector<double> massArray;
for(int i = 0 ; i < numSegment ; ++i){
SegmentInfo& segmentInfo = linkInfo.segments[i];
Matrix44 T = jointNodeSet->transforms.at(i);
DblArray3& centerOfMass = segmentInfo.centerOfMass;
CORBA::Double& mass =segmentInfo.mass;
DblArray9& inertia = segmentInfo.inertia;
TProtoFieldMap& fmap = segmentNodes[i]->fields;
copyVrmlField( fmap, "centerOfMass", centerOfMass );
copyVrmlField( fmap, "mass", mass );
copyVrmlField( fmap, "momentsOfInertia", inertia );
Vector4 c0(centerOfMass[0], centerOfMass[1], centerOfMass[2], 1.0);
Vector4 c1(T * c0);
centerOfMassArray.push_back(c1);
massArray.push_back(mass);
for(int j=0; j<3; j++){
linkInfo.centerOfMass[j] = c1(j) * mass + linkInfo.centerOfMass[j] * linkInfo.mass;
}
linkInfo.mass += mass;
if(linkInfo.mass > 0.0){
for(int j=0; j<3; j++){
linkInfo.centerOfMass[j] /= linkInfo.mass;
}
}
Matrix33 I;
I << inertia[0], inertia[1], inertia[2], inertia[3], inertia[4], inertia[5], inertia[6], inertia[7], inertia[8];
Matrix33 R;
R << T(0,0), T(0,1), T(0,2), T(1,0), T(1,1), T(1,2), T(2,0), T(2,1), T(2,2);
Matrix33 I1(R * I * R.transpose());
for(int j=0; j<3; j++){
for(int k=0; k<3; k++)
linkInfo.inertia[j*3+k] += I1(j,k);
}
segmentInfo.name = CORBA::string_dup( segmentNodes[i]->defName.c_str() );
}
if(linkInfo.mass <=0.0 )
std::cerr << "Warning: Mass is zero. <Model>" << this->name() << " <Link>" << linkInfo.name << std::endl;
for(int i = 0 ; i < numSegment ; ++i){
Vector4 c( centerOfMassArray.at(i) );
double x = c(0) - linkInfo.centerOfMass[0];
double y = c(1) - linkInfo.centerOfMass[1];
double z = c(2) - linkInfo.centerOfMass[2];
double m = massArray.at(i);
linkInfo.inertia[0] += m * (y*y + z*z);
linkInfo.inertia[1] += -m * x * y;
linkInfo.inertia[2] += -m * x * z;
linkInfo.inertia[3] += -m * y * x;
linkInfo.inertia[4] += m * (z*z + x*x);
linkInfo.inertia[5] += -m * y * z;
linkInfo.inertia[6] += -m * z * x;
linkInfo.inertia[7] += -m * z * y;
linkInfo.inertia[8] += m * (x*x + y*y);
}
}
ProcessTopologyKdTree(NeighbourRanksListType& nbRanks,
AdjacentNeighbourRanksMapType& adjNbRanks,
RankIdType processRank,
RankIdType masterRank,
std::shared_ptr<TreeType> tree,
const LeafNeighbourMapType& neighbours,
const AABB3d& aabb,
bool aligned,
const Matrix33& A_IK = Matrix33::Identity())
: m_rank(processRank)
, m_axisAligned(aligned)
, m_A_KI(A_IK.transpose())
, m_kdTree(tree.get())
, m_kdTreeRefCount(tree)
{
if (!m_kdTree)
{
GRSF_ERRORMSG("KdTree is nullptr")
}
// insert our neighbours
auto it = neighbours.find(processRank);
if (it == neighbours.end())
{
GRSF_ERRORMSG("neighbours does not contain an entry for rank: " << processRank);
}
nbRanks.insert(it->second.begin(), it->second.end());
// get adj neighbours
for (auto& rank : nbRanks)
{
std::cerr << "ProcessTopologyKdTree: NbRank: " << rank << std::endl;
// Initialize adjacent neighbour ranks to m_nbRanks for this neighbours[*it]$
auto itN = neighbours.find(rank);
if (itN == neighbours.end())
{
GRSF_ERRORMSG("neighbours does not contain an entry for rank: " << rank);
}
// build the intersection of our ranks with neighbours of other rank
adjNbRanks.emplace(rank, unorderedHelpers::makeIntersection(nbRanks, itN->second));
for (auto& v : adjNbRanks[rank])
{
std::cerr << " adjNB: " << v << std::endl;
}
}
// get our aabb
m_leaf = m_kdTree->getLeaf(m_rank);
if (!m_leaf)
{
GRSF_ERRORMSG("Trying to get leaf node for our rank: " << m_rank << " failed!")
}
std::cerr << " got leaf " << m_leaf->getIdx() << std::endl;
// adjust kdTree
// TODO (unlink childs which corresponds to subtrees we never need to check because the leafs subtree are not
// neighbours)
// not essential for correct algorithm
// but we leave the leafs which are not part of our neighbours as is, such that error messages contain these idx
// values
// if a body overlaps into a neighbour which is not part of ours
// the kdTree is already filling the whole space
// no need to expand!
// get lowest common ancestors of all boundary subtrees, including our own leaf node
// the lca node is the node we start from for collision detection
auto bndIt = m_leaf->getBoundaries().begin();
auto root = m_kdTree->getRootNode();
GRSF_ASSERTMSG(root, "Root nullptr!")
m_lcaBoundary = m_leaf;
while (bndIt != m_leaf->getBoundaries().end() && m_lcaBoundary != root)
{
if (*bndIt != nullptr)
{
m_lcaBoundary = m_kdTree->getLowestCommonAncestor(m_lcaBoundary, *bndIt);
}
++bndIt;
}
// if m_leaf = root node or , the lca becomes nullptr which is not good,
// we set it to the root node, this means we only have one leaf = 1 process = root node
if (m_lcaBoundary == nullptr)
{
m_lcaBoundary = root;
}
// collider should not add m_leaf in the results list over the overlap check
m_colliderKdTree.setNonAddedLeaf(m_leaf);
std::cerr << " finished " << std::endl;
};
bool hrp::isOrthogonalMatrix(Matrix33& m){
Matrix33 w(m * m.transpose() - Matrix33::Identity());
return (abs(w.array())<1.0e-12).all();
//return all_elements( m * m.transpose() == Matrix33::Identity() );
}
