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;
}
}
}