Пример #1
0
  void computeh(double time, BlockVector& q0, SiconosVector& y)
  {
    double height = fabs(q0.getValue(0)) - _sBallRadius;
    // std::cout <<"my_NewtonEulerR:: computeh _jachq" << std:: endl;
    // _jachq->display();
    y.setValue(0, height);
    _Nc->setValue(0, 1);
    _Nc->setValue(1, 0);
    _Nc->setValue(2, 0);
    _Pc1->setValue(0, height);
    _Pc1->setValue(1, q0.getValue(1));
    _Pc1->setValue(2, q0.getValue(2));

    //_Pc2->setValue(0,hpc);
    //_Pc2->setValue(1,data[q0]->getValue(1));
    //_Pc2->setValue(2,data[q0]->getValue(2));
    //printf("my_NewtonEulerR N, Pc\n");
    //_Nc->display();
    //_Pc1->display();
  }
Пример #2
0
  void computeh(double time, BlockVector& q0, SiconosVector& y)
  {

    std::cout <<"my_NewtonEulerR:: computeh" << std:: endl;
    std::cout <<"q0.size() = " << q0.size() << std:: endl;
    double height = q0.getValue(0) - _sBallRadius - q0.getValue(7);
    // std::cout <<"my_NewtonEulerR:: computeh _jachq" << std:: endl;
    // _jachq->display();

    y.setValue(0, height);
    _Nc->setValue(0, 1);
    _Nc->setValue(1, 0);
    _Nc->setValue(2, 0);
    _Pc1->setValue(0, q0.getValue(0) - _sBallRadius);
    _Pc1->setValue(1, q0.getValue(1));
    _Pc1->setValue(2, q0.getValue(2));

    _Pc2->setValue(0,q0.getValue(7));
    _Pc2->setValue(1,q0.getValue(8));
    _Pc2->setValue(2,q0.getValue(9));
    //printf("my_NewtonEulerR N, Pc\n");
    _Nc->display();
    _Pc1->display();
    _Pc2->display();
    std::cout <<"my_NewtonEulerR:: computeh ends" << std:: endl;
  }
Пример #3
0
void KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
  DEBUG_EXPR(q0.display());

  double X1 = q0.getValue(0);
  double Y1 = q0.getValue(1);
  double Z1 = q0.getValue(2);
  double q10 = q0.getValue(3);
  double q11 = q0.getValue(4);
  double q12 = q0.getValue(5);
  double q13 = q0.getValue(6);
  DEBUG_PRINTF("X1 = %12.8e,\t Y1 = %12.8e,\t Z1 = %12.8e,\n",X1,Y1,Z1);
  DEBUG_PRINTF("q10 = %12.8e,\t q11 = %12.8e,\t q12 = %12.8e,\t q13 = %12.8e,\n",q10,q11,q12,q13);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(q0.getNumberOfBlocks()>1)
  {
    // SP::SiconosVector x2 = _d2->q();
    // DEBUG_EXPR( _d2->q()->display(););
    X2 = q0.getValue(7);
    Y2 = q0.getValue(8);
    Z2 = q0.getValue(9);
    q20 = q0.getValue(10);
    q21 = q0.getValue(11);
    q22 = q0.getValue(12);
    q23 = q0.getValue(13);
  }
  y.setValue(0, Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(1, Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(2, Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  DEBUG_EXPR(y.display());
  DEBUG_END("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
    
}