void RTKRobotArm::copy(const MathLib::Matrix& from, Eigen::MatrixXd& to) {
int r = from.RowSize();
int c = from.ColumnSize();
for(int i=0; i<r; ++i) {
for(int j=0; j<c; ++j) {
to(i,j) = from.At(i,j);
}
}
}
/******************************************************************
* Get jacobian full direction
*
* @ J : return direction jacobian T (9 X dof)
******************************************************************/
void sKinematics::getJacobianFullDirection(int axis1, int axis2, MathLib::Matrix &J)
{
int lColSize = J.ColumnSize();
MathLib::Matrix lJ3(3,lColSize);
getJacobianPos(lJ3);
J.SetRow(lJ3.GetRow(0), 0);
J.SetRow(lJ3.GetRow(1), 1);
J.SetRow(lJ3.GetRow(2), 2);
getJacobianDirection(axis1, lJ3);
J.SetRow(lJ3.GetRow(0), 3);
J.SetRow(lJ3.GetRow(1), 4);
J.SetRow(lJ3.GetRow(2), 5);
getJacobianDirection(axis2, lJ3);
J.SetRow(lJ3.GetRow(0), 6);
J.SetRow(lJ3.GetRow(1), 7);
J.SetRow(lJ3.GetRow(2), 8);
}
// Read sensor data and prepare SFinger data structure
void HandSkinControllerThread::PrepareSFingers(){
bool bdm = bDebugMode;
bDebugMode = false;
if(bDebugMode){
cout << "Input *************"<<endl;
}
// Position and default command
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerPos[i][0] = mJointsPos[9+i*2 +0];
mSFingerPos[i][1] = mJointsPos[9+i*2 +1];
mSFingerCmd[i][0] = mJointsPos[9+i*2 +0];
mSFingerCmd[i][1] = mJointsPos[9+i*2 +1];
}
static double badSkinVal[12];
double tmpSkinVal[12];
if(bBadSkin){
// Let simulate some bad skin values from time to time
//double period = 0.2;
//double off = mTime/period - floor(mTime/period);
//if(off < 0.05){
for(int j=0;j<12;j++){
badSkinVal[j] = 9.0+4.0*(rand()%1000)/1000.0;
}
//}
for(int i=0;i<FINGERS_COUNT;i++){
if(i==mBadFingerId){
int offset = (i==0?4:i-1)*12;
for(int j=0;j<12;j++){
tmpSkinVal[j] = mFingerTip[offset+j];
mFingerTip[offset+j] = badSkinVal[j];
}
}
}
}
// Compute the average fingerTip pressure
for(int i=0;i<FINGERS_COUNT;i++){
int offset = (i==0?4:i-1)*12;
double val = 0.0;
for(int j=0;j<12;j++){
val += mFingerTip[offset+j];
}
val /= double(12);
mSFingerTip[i][0] = val;
}
if(bDebugMode){
cout << "Input Pressure: "<< mSFingerTip[0][0]<<" "<<mSFingerTip[1][0]<<" "<<mSFingerTip[2][0]<<endl;
}
// FingerTip normal direction
double vals[]={
-1.00000, 0.00000, 0.00000,
-0.39956, 0.00000, 0.91671,
-0.39956, 0.00000, 0.91671,
-1.00000, 0.00000, 0.00000,
-0.78673, 0.60316, 0.13140,
-0.30907, 0.47765, 0.82239,
-0.00000, 1.00000, 0.00000,
0.30907, 0.47765, 0.82239,
0.78673, 0.60316, 0.13140,
1.00000, 0.00000, 0.00000,
0.39956, 0.00000, 0.91671,
0.39956, 0.00000, 0.91671
};
MathLib::Matrix fingerTipPDir;
fingerTipPDir.Set(vals,12,3);
// Compute the normal direction
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerTipDir[i].Zero();
int offset = (i==0?4:i-1)*12;
double val = 0.0;
for(int j=0;j<12;j++){
val += mFingerTip[offset+j];
mSFingerTipDir[i] += fingerTipPDir.GetRow(j) *(mFingerTip[offset+j]);
}
if(val>1.0){
mSFingerTipDir[i].Normalize();
}else{
mSFingerTipDir[i].Zero();
}
}
if(bBadSkin){
// Revert original skin values if necessary for data saving
for(int i=0;i<FINGERS_COUNT;i++){
if(i==mBadFingerId){
int offset = (i==0?4:i-1)*12;
for(int j=0;j<12;j++){
mFingerTip[offset+j] = tmpSkinVal[j];
}
}
}
}
if(bDebugMode){
cout << "Input Direction: "<< mSFingerTipDir[0]<<" "<<mSFingerTipDir[1]<<" "<<mSFingerTipDir[2]<<endl;
}
bDebugMode = bdm;
return;
}