ofxRobotArm/src/kinematics/RelaxedIKSolver.cpp at beta.1 · CreativeInquiry/ofxRobotArm · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
//
//  Solver.cpp
//  example-simple
//
//  Created by Dan Moore on 1/2/21.
//

#include "RelaxedIKSolver.h"
#include "RelaxedIK.hpp"
using namespace ofxRobotArm;

RelaxedIKSolver::RelaxedIKSolver(){
    ofLog()<<"RelaxedIKSolver THREAD"<<endl;
    vector<double> foo(6., 0.0);
    currentPose.setup(foo);
    currentPose.getBack().assign(6, 0.0);
    currentPose.swapBack();
    frameNum = 0;
}

RelaxedIKSolver::~RelaxedIKSolver(){
    stop();
    waitForThread(false);
}

void RelaxedIKSolver::start(){
    startThread();
}
void RelaxedIKSolver::stop(){
    stopThread();
}
void RelaxedIKSolver::startThread(){
    bThreadStarted = true;
    ofThread::startThread();
}
void RelaxedIKSolver::stopThread(){
    if(isThreadRunning()){
        bThreadStarted = false;
        ofThread::stopThread();
    }
}
void RelaxedIKSolver::setPose(Pose desiredPose, Pose actualPose){
    lock();
    this->desiredPose = desiredPose;
    this->actualPose = actualPose;
    unlock();
}
vector<double> RelaxedIKSolver::getCurrentPose(){
    vector<double> ret;
    lock();
    currentPose.swapFront();
    ret = currentPose.getFront();;
    unlock();
    return ret;
}

void RelaxedIKSolver::setInitialPose(vector<double> pose){
    lock();
    // set_starting_config(pose.data(), pose.size());
    unlock();
}

void RelaxedIKSolver::threadedFunction(){
    while(isThreadRunning()){
        lock();

        ofVec3f difPos = (desiredPose.position - actualPose.position);
        ofQuaternion rot  = (actualPose.orientation * desiredPose.orientation);
        ofVec4f r = ofVec4f(rot.x(), rot.y(), rot.z(), rot.w());

        std::vector<double> pos(3, 0.0);
        pos[0] = difPos.x;
        pos[1] = difPos.y;
        pos[2] = difPos.z;

        std::vector<double> quat(4, 0.0);
        quat[0] = r.x;
        quat[1] = r.y;
        quat[2] = r.z;
        quat[3] = r.w;

        Opt x = solve(pos.data(), (int) pos.size(), quat.data(), (int) quat.size());
        for (int i = 0; i < x.length; i++) {
            currentPose.getBack()[i] = x.data[i];
        }
        frameNum++;
        if(frameNum > 4000)
            frameNum = 0;
        currentPose.swapBack();
        unlock();

        ofSleepMillis(1);
    }
}

bool RelaxedIKSolver::isThreadRunning(){
    bool ret;
    lock();
    ret = bThreadStarted;
    unlock();
    return ret;
}

uint64_t RelaxedIKSolver::getFrame(){
    uint64_t t = 0;
    lock();
    frameAvg +=frameNum;
    frameAvg /=2;
    t = frameAvg;
    frameNum = 0;
    unlock();
    return t;
}