KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
std::vector<std::string> la_joints;
la_joints.push_back("left_shoulder_pitch");
la_joints.push_back("left_shoulder_roll");
la_joints.push_back("left_arm_ft_sensor");
la_joints.push_back("left_shoulder_yaw");
la_joints.push_back("left_elbow");
la_joints.push_back("left_wrist_prosup");
la_joints.push_back("left_wrist_pitch");
la_joints.push_back("left_wrist_yaw");
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
std::cout << kdlChain << std::endl;
int nj = idynChain.getN();
//cout << "idynChainGet_usingKDL_aux with sensor" << " nrJoints " << kdlChain.getNrOfJoints() << " nrsegments " << kdlChain.getNrOfSegments() << endl;
assert(nj==kdlChain.getNrOfJoints());
assert(nj+1==kdlChain.getNrOfSegments());
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
return f_int;
}
int main_cfunc(void) {
// your code goes here
int *p;
int i = 30;
p = &i;
f_int(p);
f_void_int(p);
f_void_void(p);
return 0;
}
yarp::sig::Vector idynChainGetTorques_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain);
int nj = idynChain.getN();
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
yarp::sig::Vector ret_tau;
kdlJntArray2idynVector(torques,ret_tau);
return ret_tau;
}
PyObject *unary_af( PyObject *ary, F_SHORT f_short, F_INT f_int, F_LONG f_long, F_DOUBLE f_double ) {
try {
PyArrayObject *array = ( PyArrayObject * ) PyArray_FROM_O( ary );
switch( array->descr->type_num ) {
case NPY_SHORT:
case NPY_USHORT:
return f_short( array );
case NPY_UINT:
case NPY_INT:
return f_int( array );
case NPY_LONG:
case NPY_ULONG:
return f_long( array );
case NPY_FLOAT:
case NPY_DOUBLE:
return f_double( array );
default:
throw npp::PyException( PyExc_TypeError, "not a numeric array" );
}
} catch( npp::PyException & ) {
return NULL;
}
}
