← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Body/CompositeIK.cpp

  • Committer: Thomas Moulard
  • Date: 2012-10-23 12:43:24 UTC
  • Revision ID: git-v1:351cf736ad49bc7a9a7b9767dee760a013517a5d
Tags: upstream/1.1.0
Imported Upstream version 1.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Body/CompositeIK.cpp
 
1
/*!
 
2
  @file
 
3
  @author Shin'ichiro Nakaoka
 
4
*/
 
5
 
 
6
#include "CompositeIK.h"
 
7
#include "Link.h"
 
8
#include "JointPath.h"
 
9
 
 
10
using namespace std;
 
11
using namespace boost;
 
12
using namespace cnoid;
 
13
 
 
14
 
 
15
CompositeIK::CompositeIK(BodyPtr body, Link* targetLink)
 
16
{
 
17
    body_ = body;
 
18
    targetLink_ = targetLink;
 
19
    isAnalytical_ = false;
 
20
}
 
21
 
 
22
 
 
23
CompositeIK::~CompositeIK()
 
24
{
 
25
 
 
26
}
 
27
 
 
28
 
 
29
bool CompositeIK::addBaseLink(Link* baseLink)
 
30
{
 
31
    if(baseLink && targetLink_){
 
32
        JointPathPtr path = body_->getJointPath(targetLink_, baseLink);
 
33
        if(path){
 
34
            isAnalytical_ = pathList.empty() ? path->hasAnalyticalIK() : (isAnalytical_ && path->hasAnalyticalIK());
 
35
            PathInfo info;
 
36
            info.path = path;
 
37
            info.p0 = baseLink->p;
 
38
            info.R0 = baseLink->R;
 
39
            pathList.push_back(info);
 
40
            return true;
 
41
        }
 
42
    }
 
43
    return false;
 
44
}
 
45
 
 
46
 
 
47
void CompositeIK::setMaxIKerror(double e)
 
48
{
 
49
    for(size_t i=0; i < pathList.size(); ++i){
 
50
        pathList[i].path->setMaxIKerror(e);
 
51
    }
 
52
}
 
53
 
 
54
 
 
55
bool CompositeIK::hasAnalyticalIK() const
 
56
{
 
57
    return isAnalytical_;
 
58
}
 
59
 
 
60
 
 
61
bool initialize();
 
62
 
 
63
 
 
64
bool CompositeIK::calcInverseKinematics(const Vector3& p, const Matrix3& R)
 
65
{
 
66
    const int n = body_->numJoints();
 
67
 
 
68
    Vector3 p0 = targetLink_->p;
 
69
    Matrix3 R0 = targetLink_->R;
 
70
    std::vector<double> q0(n);
 
71
    for(int i=0; i < n; ++i){
 
72
        q0[i] = body_->joint(i)->q;
 
73
    }
 
74
 
 
75
    targetLink_->p = p;
 
76
    targetLink_->R = R;
 
77
    bool solved = true;
 
78
    for(size_t i=0; i < pathList.size(); ++i){
 
79
        PathInfo& info = pathList[i];
 
80
        solved = info.path->calcInverseKinematics(p, R, info.p0, info.R0);
 
81
        if(!solved){
 
82
            break;
 
83
        }
 
84
        Link* endLink = info.path->endLink();
 
85
        endLink->p = info.p0;
 
86
        endLink->R = info.R0;
 
87
    }
 
88
 
 
89
    if(!solved){
 
90
        targetLink_->p = p0;
 
91
        targetLink_->R = R0;
 
92
        for(int i=0; i < n; ++i){
 
93
            body_->joint(i)->q = q0[i];
 
94
        }
 
95
        for(size_t i=0; i < pathList.size(); ++i){
 
96
            PathInfo& info = pathList[i];
 
97
            info.path->calcForwardKinematics();
 
98
            Link* endLink = info.path->endLink();
 
99
            endLink->p = info.p0;
 
100
            endLink->R = info.R0;
 
101
        }
 
102
    }
 
103
 
 
104
    return solved;
 
105
}