SICONOS_EXPORT void mass(unsigned int sizeOfq, const double *q, double *mass, unsigned int sizeZ, double* z)
{
int n = sizeOfq;
int n1 = n*n;
// mass set to zero
for (int i = 0; i < n1; i++)
{
mass[i] = 0.0;
}
Inertia(mass, q);
}
// hDisplay is passed because that's the input of all keystrokes.
// we can count on being focused... not much point in
// responding to ALL keystrokes ALL the time...
void ScanKeyboard( PRENDERER hDisplay, PVECTOR KeySpeed, PVECTOR KeyRotation )
{
PVECTOR v;
RCOORD max, step;
//if( !hDisplay )
// return;
// how does this HOOK into the camera update functions?!
#define Relax(V, n) if( (V[n]>0.001) || ( V[n]<-0.001) ) \
(V[n] /= 2.0f); else (V[n] = 0);
#define Accel(V, n) \
if ( V[n] < max ) \
V[n] += step; \
else V[n] = max;
#define Decel(V,n) \
if( V[n] > -max ) \
V[n] -= step; \
else V[n] = -max;
#define ROTATION (v=KeyRotation, \
max = ROTATION_DELTA, \
step=ROTATION_ACCEL)
#define SPEED (v=KeySpeed, \
max = KEY_VELOCITY, \
step = KEY_ACCEL )
#define Inertia( KEY_LESS, KEY_MORE,KEY_LESS2, KEY_MORE2, va ) \
if( IsKeyDown(hDisplay, KEY_LESS ) || IsKeyDown(hDisplay, KEY_LESS2) ) Decel(v, va) \
else if( IsKeyDown(hDisplay, KEY_MORE ) || IsKeyDown(hDisplay, KEY_MORE2) ) Accel(v, va) \
else Relax( v, va );
(ROTATION);
Inertia(KEY_DOWN, KEY_UP, KEY_F, KEY_R, vRight );
Inertia(KEY_LEFT, KEY_RIGHT, KEY_A, KEY_D, vUp );
Inertia(KEY_PGUP, KEY_HOME, 0, 0, vForward );
(SPEED);
Inertia( KEY_END, KEY_PGDN, KEY_Q, KEY_E, vRight );
Inertia( KEY_CENTER, KEY_ENTER, KEY_G, KEY_T, vUp );
Inertia( KEY_DELETE, KEY_INSERT, KEY_S, KEY_W, vForward );
}
void vpSiloWorld::Create(void)
{
vpMaterial::GetDefaultMaterial()->SetRestitution(0.3);
vpMaterial::GetDefaultMaterial()->SetDynamicFriction(0.3);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
block[i][j].AddGeometry(new vpBox(Vec3(1, 2, 1)));
block[i][j].SetFrame(Exp(se3(0, 0, (scalar)(j + (scalar)0.5 * (i % 2)) / (scalar)NUM_SILO_BLOCKS * M_2PI, 0, 0, 0)) * SE3(Vec3(4.4, 0, 1.01 * i)));
AddBody(&block[i][j]);
block[i][j].SetInertia(Inertia(1));
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
ball[i].AddGeometry(new vpSphere(0.25));
ball[i].SetFrame(Vec3(drand(2.0), drand(2.0), NUM_SILO_LEVEL + (25.0 / NUM_BALL) * i));
ball[i].SetGenVelocity(se3(0,0,0,drand(0.1), drand(0.1), drand(0.1)));
ball[i].SetInertia(Inertia(0.5));
AddBody(&ball[i]);
}
floor.AddGeometry(new vpBox(Vec3(100, 100, 1)));
floor.SetFrame(Vec3(0, 0, -1.0));
floor.SetGround();
AddBody(&floor);
roof.AddGeometry(new vpBox(Vec3(10, 10, 1)), Vec3(0.0, 0.0, -1));
roof.SetFrame(Vec3(0, 0, -10));
AddBody(&roof);
//SetIntegrator(VP::IMPLICIT_EULER);
SetIntegrator(VP::EULER);
SetTimeStep(0.005);
SetGravity(Vec3(0.0, 0.0, -10.0));
Initialize();
BackupState();
renderer.SetTarget(this);
if ( materialArray.size() )
{
renderer.SetMaterial(&floor, materialArray[0]);
renderer.SetMaterial(&roof, materialArray[1]);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
renderer.SetMaterial(&block[i][j], materialArray[2]);
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
renderer.SetMaterial(&ball[i], materialArray[1]);
}
}
}
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
int ndof;
int contacts;
char lagrangianModelName[20] = "";
getLagrangianModelName(lagrangianModelName);
getActiveDOFsize(&ndof);
vector<int> activeDOF(ndof);
if (ndof > 0)
getActiveDOF(&(activeDOF[0]));
else
ndof = *NDOF;
///////////////////////////////////////////
// Computation of the Lagrangian model
///////////////////////////////////////////
matrix<double, column_major> M(*NDOF, *NDOF);
vector<double> N(*NDOF);
if (strcmp(lagrangianModelName, "Human36") == 0)
{
double L[31];
double addl[84];
double mass;
GetModelMass(&mass);
GetAnatomicalLengths(L);
GetTag2JointLengths(addl);
InertiaH36(&(M(0, 0)), q, L, addl, mass);
NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
}
else
{
Inertia(&(M(0, 0)), q);
NLEffects(&(N[0]), q, qdot);
}
//////////////////////////////////////////////////
//Additionnal forces
/////////////////////////////////////////////////
vector<double> fAdditionnal(*NDOF);
if (strcmp(lagrangianModelName, "PA10") == 0)
Friction(&(fAdditionnal[0]), q, qdot);
else if (strcmp(lagrangianModelName, "RX90") == 0)
SpringForce(&(fAdditionnal[0]), q);
else
fAdditionnal.clear();
///////////////////////////////////////////////////
// Limitation to the selected degrees of freedom
///////////////////////////////////////////////////
reduceMatrix(M, activeDOF, activeDOF);
N = reduceVector(N, activeDOF);
fAdditionnal = reduceVector(fAdditionnal, activeDOF);
//////////////////////////////////////////////////
// Proportionnel-derivee in the task space
//////////////////////////////////////////////////
vector<double> sDesired(*NDOF);
vector<double> sdotDesired(*NDOF);
matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);
//vector<double> sddotDesired(*NDOF);
trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);
//v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
vector<double> TF(*NDOF);
vector<double> h(*NDOF);
matrix<double, column_major> H(*NDOF, *NDOF);
TaskFunction(&(TF[0]), q);
TaskJacobian(&(H(0, 0)), q);
TaskNLEffects(&(h[0]), q, qdot);
double Kp = 100.0;
double Kv = 30.0;
vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;
/////////////////////////////////////////////////////////////
// Generalized Torques to make the realisation of the command
/////////////////////////////////////////////////////////////
//qddot = inv(H)*v;
vector<int> ipiv(*NDOF);
// lapack::getrf(H, ipiv);
// sddotDesired = prod(H, sddotDesired);
lapack::gesv(H, ipiv, sddotDesiredMATRIX);
//D = M*qddot(ActiveDOF) + N + FAdditionnal;
sddotDesired = reduceVector(sddotDesired, activeDOF);
N += prod(M, sddotDesired) + fAdditionnal;
//nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
//Torques = zeros(q);
//Torques(ActiveDOF(1:nmot)) = D(1:nmot);
for (int i = 0; i < *NDOF; i++)
torques[i] = 0;
expandVector(torques, N, activeDOF);
}
RBGroundBody()
: ImmobileRigidBodyNode(MassProperties(Infinity, Vec3(0), Inertia(Infinity)),
Transform(), Transform(),
UIndex(0), USquaredIndex(0), QIndex(0)) {}
void ScanKeyboard( PRENDERER hDisplay, PVECTOR KeySpeed, PVECTOR KeyRotation )
{
PVECTOR v;
RCOORD max, step;
if( !hDisplay )
return;
if( !T )
T = CreateTransform();
// how does this HOOK into the camera update functions?!
if( IsKeyDown((hDisplay), KEY_X) || IsKeyDown( (hDisplay), KEY_Q ) )
{
extern int gbExitProgram;
gbExitProgram = TRUE;
}
if( pFirstObject )
{
if( KeyDown( (hDisplay), KEY_A ) )
{
PFACETSET pps;
VECTOR v;
RotateAbs( T, 0.1f, 0, 0 );
pps = pFirstObject->pPlaneSet;
ApplyRotation( T, v, pps->pPlanes->d.n );
SetPoint( pps->pPlanes->d.n, v );
IntersectPlanes( pFirstObject->pLinePool,
pps, TRUE );
}
if( KeyDown( (hDisplay), KEY_Z ) )
{
PFACETSET pps;
VECTOR v;
RotateAbs( T, -0.1f, 0, 0 );
pps = pFirstObject->pPlaneSet;
ApplyRotation( T, v, pps->pPlanes->d.n );
SetPoint( pps->pPlanes->d.n, v );
IntersectPlanes( pFirstObject->pLinePool,
pps, TRUE );
}
}
#define Relax(V, n) if( (V[n]>0.001) || ( V[n]<-0.001) ) \
(V[n] /= 2.0f); else (V[n] = 0);
#define Accel(V, n) \
if ( V[n] < max ) \
V[n] += step; \
else V[n] = max;
#define Decel(V,n) \
if( V[n] > -max ) \
V[n] -= step; \
else V[n] = -max;
#define ROTATION (v=KeyRotation, \
max = ROTATION_DELTA, \
step=ROTATION_ACCEL)
#define SPEED (v=KeySpeed, \
max = KEY_VELOCITY, \
step = KEY_ACCEL )
#define Inertia( KEY_LESS, KEY_MORE,KEY_LESS2, KEY_MORE2, va ) \
if( IsKeyDown(hDisplay, KEY_LESS ) || IsKeyDown(hDisplay, KEY_LESS2) ) Decel(v, va) \
else if( IsKeyDown(hDisplay, KEY_MORE ) || IsKeyDown(hDisplay, KEY_MORE2) ) Accel(v, va) \
else Relax( v, va );
(ROTATION);
Inertia(KEY_DOWN, KEY_UP, 0, 0, vRight );
Inertia(KEY_RIGHT, KEY_LEFT, 0, 0, vUp );
Inertia(KEY_PGUP, KEY_HOME, 0, 0, vForward );
(SPEED);
Inertia( KEY_END, KEY_PGDN, 0, 0, vRight );
Inertia( KEY_ENTER, KEY_GRAY_PLUS, 0, KEY_CENTER, vUp );
Inertia( KEY_DELETE, KEY_INSERT, 0, 0, vForward );
}
void Critter::Update(float dt) {
UpdateInternal(dt);
Accelerate(dt);
Inertia(dt);
}
SICONOS_EXPORT void mass(unsigned int sizeOfq, const double *q, double *Mass, unsigned int sizeZ, double* z)
{
// compute mass matrix
Inertia(Mass, q);
}
