vctFrame4x4<double> robSAHThumb::ForwardKinematics( const vctFixedSizeVector<double,4>& q, robSAHThumb::Phalanx phalanx ) const { switch( phalanx ){ case robSAHThumb::BASE: return Rtw0; case robSAHThumb::METACARPUS: { vctMatrixRotation3<double> Rb0; vctFixedSizeVector<double,3> tb0( -3.0/1000.0, 27.1/1000.0, 0.0 ); vctFrame4x4<double> Rtb0(Rb0,tb0); vctMatrixRotation3<double> Rb1( 0.00000, -1, 0.00000, 0.57358, 0, -0.81915, 0.81915, 0, 0.57358, VCT_NORMALIZE ); vctFixedSizeVector<double,3> tb1( -9.0/1000.0, 114.0/1000.0, 97.0/1000.0); vctFrame4x4<double> Rtb1( Rb1, tb1 ); vctFrame4x4<double> Rt0b( Rtb0 ); Rt0b.InverseSelf(); vctFrame4x4<double> Rt01 = Rt0b * Rtb1; vctMatrixRotation3<double> R( cos( -q[0] ), -sin( -q[0] ), 0.0, sin( -q[0] ), cos( -q[0] ), 0.0, 0.0, 0.0, 1.0, VCT_NORMALIZE ); vctFixedSizeVector<double,3> t(0.0); vctFrame4x4<double> Rt( R, t ); return ForwardKinematics( q, robSAHThumb::BASE ) * Rtb0 * Rt * Rt01; } case robSAHThumb::MCP: { return ( ForwardKinematics( q, robSAHThumb::METACARPUS ) * links[0].ForwardKinematics( q[1] ) ); } case robSAHThumb::PROXIMAL: return ( ForwardKinematics( q, robSAHThumb::MCP ) * links[1].ForwardKinematics( q[2] ) ); case robSAHThumb::INTERMEDIATE: return ( ForwardKinematics( q, robSAHThumb::PROXIMAL ) * links[2].ForwardKinematics( q[3] ) ); case robSAHThumb::DISTAL: return ( ForwardKinematics( q, robSAHThumb::INTERMEDIATE ) * links[3].ForwardKinematics( q[3] ) ); } }
void SimpleSkeletalAnimatedObject_cl::SetMode(SampleMode_e newMode) { ClearData(); m_eSampleMode = newMode; switch (newMode) { case MODE_SINGLEANIM: StartSingleAnimation(false); break; case MODE_BLENDTWOANIMS: BlendTwoAnimations(); break; case MODE_LISTENTOEVENTS: ListenToEvents(); break; case MODE_SETANIMATIONTIME: SetAnimationTime(); break; case MODE_LAYERTWOANIMATIONS: LayerTwoAnimations(); break; case MODE_FORWARDKINEMATICS: ForwardKinematics(); break; case MODE_LOOKAT: LookAt(); break; } // disable motion delta for this sample if (GetAnimConfig() != NULL) GetAnimConfig()->SetFlags(GetAnimConfig()->GetFlags() & ~APPLY_MOTION_DELTA); }
bool osaGLUTManipulator::SetPositions(const vctDoubleVec& q) { // Ensure one joint value per link if (q.size() != links.size()) { CMN_LOG_RUN_ERROR << "osaGLUTManipulator::SetPositions: expected " << links.size() << " values, got " << q.size() << " values." << std::endl; return false; } this->q = q; for( size_t i=0; i < meshes.size(); i++ ){ vctFrame4x4<double> Rtwi = ForwardKinematics( q, i+1 ); meshes[i]->SetPositionOrientation( Rtwi ); } return true; }
void executeControl() { //currentPosition = GetPosition(1); //currentPosition = ArmGetPosition(); for(int i = 0; i<DOF;i++) { if(doReading) { currentPosition[i] = GetPosition(i); if(abs(currentPosition[i] - lastPosition[i])>0.5) { currentPosition[i] = (lastPosition[i] + differentialPosition[i]); //printf("Current = %f %f %f \n",currentPosition[0],currentPosition[1],currentPosition[2]); //PORTC ^= 0b0100000; } ForwardKinematics(); } if(doControl && doReading) { e[i] = desiredPosition[i] - currentPosition[i]; ei[i] = ei[i] + e[i]; tau[i] = (kp[i] * (e[i])) + (ki[i] * ei[i]); //_delay_ms(1); } lastPosition[i] = currentPosition[i]; differentialPosition[i] = currentPosition[i] - lastPosition[i]; } if(doControl && doReading) { ArmTorque(tau); } }
void executeControl() { // READING if(doReading == 1 && doControl == 0) { updateQPosition(); //for(int i = 0; i<DOF;i++) //{ //currentQPosition[i] = GetPosition(i); //if(abs(currentQPosition[i] - lastPosition[i])>0.5) //{ //currentQPosition[i] = (lastPosition[i] + differentialPosition[i]); //} // ESTIMATE NEXT STEP //lastPosition[i] = currentQPosition[i]; //differentialPosition[i] = currentQPosition[i] - lastPosition[i]; //_delay_us(500); //_delay_ms(1); //} //printf("Test g 1 %f %f %f %f %f %f %f \r\n",currentQPosition[0],currentQPosition[1],currentQPosition[2],currentQPosition[3],currentQPosition[4],currentQPosition[5],currentQPosition[6]); ForwardKinematics(); if(instructionSet == 1) { printf("p 1 %f %f %f %f %f %f %f \r", currentXPosition[0],currentXPosition[1],currentXPosition[2],currentXPosition[3],currentXPosition[4],currentXPosition[5],currentXPosition[6]); instructionSet = 0; } if(instructionSet == 2) { printf("q 1 %f %f %f %f %f %f %f \r", currentQPosition[0],currentQPosition[1],currentQPosition[2],currentQPosition[3],currentQPosition[4],currentQPosition[5],currentQPosition[6]); instructionSet = 0; } } if(doControl) { float nextPos[DOF]; float t; //Aqui va ir la evaluaci髇 de la trayectoria y el env韔 de datos. El prec醠culo de las constantes de la planeaci髇 se hacen en cmdATM al recibir los datos en la instrucci髇 p t = timeCounter * sampleTime; //printf("ExecutionTime = %f \n",executionTF); //printf("time = %f \n",t); for(int i = 0; i<DOF;i++) { nextPos[i] = perQuintico(initialQPosition[i],desiredQPosition[i],PERcoef[i],t); //nextPos[i] = evalLSPB(initialQPosition[i],desiredQPosition[i],LSPBcoef[i],t); currentQPosition[i] = GetPosition(i); //if(abs(currentQPosition[i] - nextPos[i])>maxTol) //currentQPosition[i] = nextPos[i]; } ArmPosition(nextPos); //printf("%f , %f , %f , %f , %f , %f , %f , %f \n", t, nextPos[0],nextPos[1],nextPos[2],nextPos[3],nextPos[4],nextPos[5],nextPos[6]); _delay_ms(5); timeCounter++; if(t >= executionTF) { doControl = 0; } } }