多自由度机械臂模拟5

机械臂-起臂动作模拟

double cAngle = calaulateCAngle();
std::cout << "rorate cAngle:" << cAngle << std::endl;

for (double d1=0.0;d1<cAngle;d1+=2.0)
{
    if (cAngle-d1>2.0)
    {
        rorate3 += 2.0;
        rotateRope = -2.0;
    }
    else 
    {
        double d_1 = cAngle - d1;
        rorate3 += d_1;
        rotateRope = -d_1;
    }
    rotate3();
    rotateRope1();
    printHookPosition();
    OpenThreads::Thread::microSleep(1 * 50000);
    viewer1->frame();
}