IKResult PathPlanner::checkForValidConfigurations(IKResult configs){
IKResult result;
matrix<double> jointPosition(4, 4);
int value = 0;
double maxWrist1Angle = 0 * PI / 180;
double minWrist1Angle = -180 * PI / 180;
double maxWrist2Angle = 90 * (PI / 180);
double minWrist2Angle = -maxWrist2Angle;
for (int i = 0; i < configs.solutions.size(); i++) {
value = 0;
if (minWrist1Angle <= configs.solutions[i].getWrist1Angle() && configs.solutions[i].getWrist1Angle() <= maxWrist1Angle /*&& minWrist2Angle <= configs.solutions[i].getWrist2Angle() && configs.solutions[i].getWrist2Angle() <= maxWrist2Angle*/)
{
for (int j = 0; j < 6; j++) {
jointPosition = direct_kinematics_.getPositionOfJoint(j, configs.solutions[i]);
//If the z coordinate of the joint is above the table
if (jointPosition(2, 3) > 0.05) {
value++;
}
}
if (value == 6){
result.solutions.push_back(configs.solutions[i]);
result.configuration.push_back(configs.configuration[i]);
}
}
}
return result;
}
vector<double> CNR_7DOFAnalyticInverseKinematicsComp::GetJointPosition()
{
vector<double> jointPosition(7);
// user code here
MSLVector EE(m_Position.x, m_Position.y, m_Position.z); //앤드이펙터 포지션
double cr, cp , cy;
double sr, sp , sy;
cr = cos(DEG2RAD(m_Position.roll)); sr = sin(DEG2RAD(m_Position.roll)); //roll값 적용
cp = cos(DEG2RAD(m_Position.pitch)); sp = sin(DEG2RAD(m_Position.pitch)); //pitch값적용
if(m_Position.yaw == 0){//yaw값이 0이면 해가 안구해져서 이렇게 처리
cy = cos(1e-011); sy = sin(1e-011); //yaw값 적용
} else {
cy = cos(DEG2RAD(m_Position.yaw)); sy = sin(DEG2RAD(m_Position.yaw)); //yaw값 적용
}
double uplen = atof(parameter.GetValue("UpperArmLength").c_str()); //상완길이 파라미터로부터 설정해야함
double lowlen = atof(parameter.GetValue("LowerArmLength").c_str()); //하완길이 파라미터로부터 설정해야함
double handlen = atof(parameter.GetValue("ToolLength").c_str()); //손목길이 파라미터로부터 설정해야함
double ax_o,ay_o,az_o,ox_o,oy_o,oz_o,nx_o,ny_o,nz_o;//원래의 프레임 방향인자
nx_o = cy*cp; ox_o = cy*sp*sr-sy*cr; ax_o = cy*sp*cr+sy*sr;
ny_o = sy*cp; oy_o = sy*sp*sr+cy*cr; ay_o = sy*sp*cr-cy*sr;
nz_o = -sp; oz_o = cp*sr; az_o = cp*cr;
double px_o, py_o,pz_o; // 원래의 손목위치;
px_o = EE[0] - handlen * ax_o; //손의 길이만큼 목표위치를 빼서 손목의 위치를 계산
py_o = EE[1] - handlen * ay_o;
pz_o = EE[2] - handlen * az_o;
MSLVector W(3);
W[0] = py_o; //손목의 위치를 알고리즘에게 맞게 변환해서 입력
W[1] = -pz_o;
W[2] = -px_o;
if(W[0] == 0 && W[1] == 0)
W[0] += 1e-011;//x,y가 둘다 0이면 해가 안구해져서 이렇게 처리
double nx,ny,nz,ox,oy,oz,ax,ay,az;
//회전축 n,o,a를 알고리즘 방향기준에 맞게 변환
nx = ny_o;
ox = oy_o;
ax = ay_o;
ny = -nz_o;
oy = -oz_o;
ay = -az_o;
nz = -nx_o;
oz = -ox_o;
az = -ax_o;
double wlen = W.length(); //손목까지의 거리
MSLVector P(0.0, 0.0, 100.0); //첫번째 관절벡터
double len1 = uplen; //상완길이
double len2 = lowlen; //하완길이
MSLVector SCP, C;
SCP = (P*W)/(W*W)*W; //scp는 shoulder 점에서 p벡터의 w벡터에 대한 수선벡터의 베이스까지향하는 벡터이다. (scp+ (p벡터의 w벡터에 대한 수선벡터) = p벡터)
C = P - SCP;
double v,omega,dtemp;
double rx, ry, rz, so,co;
omega = DEG2RAD(atof(parameter.GetValue("RedundantValue").c_str())); //여자유도 변수 적용
v = 1-cos(omega);
dtemp = sqrt(W[0]*W[0] + W[1]*W[1] +W[2]*W[2]);
rx = W[0]/dtemp;
ry = W[1]/dtemp;
rz = W[2]/dtemp;
so = sin(omega);
co = cos(omega);
MSLMatrix R(3,3);
R[0][0] = rx * rx * v + co;
R[0][1] = rx * ry * v - rz * so;
R[0][2] = rx *rz*v + ry * so;
R[1][0] = rx * ry * v + rz * so;
R[1][1] = ry * ry * v + co;
R[1][2] = ry * rz * v - rx * so;
R[2][0] = rx * rz * v - ry * so;
R[2][1] = ry * rz * v + rx * so;
R[2][2] = rz * rz * v + co;
C = R * C; //c벡터를 손목 회전벡터로 오메가각도만큼 회전
C = C.norm() ; //c를 노말라이징
double he;
he = 0.5 * (wlen + uplen + lowlen);
he = sqrt(he*(he - wlen)*(he - lowlen)*(he - uplen));
double clen = 2.0 * he / wlen;
C = clen * C; //W벡터 중간정도에서 엘보까지 가는 벡터 완성
MSLVector SE; //어깨에서 엘보까지 벡터
SE = (W/wlen)*sqrt(uplen*uplen - clen*clen) + C;
MSLVector EW; //엘보에서 손목까지 벡터
EW = W - SE;
double th1, th2, th3, th4, th5, th6, th7;
double c1,s1,c2,s2,c3,s3,c4,s4,c5,s5,c6,s6;
th2 = acos(SE[1]/uplen);
c2 = cos(th2);
s2 = sin(th2);
double ss,cc;
ss = -SE[2]/(uplen*sin(th2));
cc = -SE[0]/(uplen*sin(th2));
th1 = atan2(ss,cc);
c1 = cos(th1);
s1 = sin(th1);
th4 = acos( (SE*EW)/((SE.length())*(EW.length())) );
c4 = cos(th4);
s4 = sin(th4);
double AA,BB,CC;
AA = -EW[0]/lowlen - c1*s2*c4;
BB = -EW[2]/lowlen - s1*s2*c4;
ss = (c1*BB - s1*AA)/s4;
cc = (-EW[1]/lowlen + c2*c4)/(-s2*s4);
th3 = atan2(ss,cc);
c3 = cos(th3);
s3 = sin(th3);
MSLVector RHZ(ax, ay, az);
th6 = acos( (EW*RHZ)/lowlen);
c6 = cos(th6);
s6 = sin(th6);
AA = -ax -(-(c1*c2*c3+s1*s3)*s4+c1*s2*c4)*c6;
BB = -ay -(-s2*c3*s4-c2*c4)*c6;
CC = (-c1*c2*s3+s1*c3)*(s2*c3*c4-c2*s4) - (-s2*s3)*((c1*c2*c3+s1*s3)*c4+c1*s2*s4);
cc = ( (-s2*s3) *(AA) - (-c1*c2*s3+s1*c3) *(BB) )/((CC)*s6 );
ss = -( (s2*c3*c4-c2*s4) *(AA)- ((c1*c2*c3+s1*s3)*c4+c1*s2*s4) *(BB)) /((CC)*s6);
th5 = atan2(ss,cc);
c5 = cos(th5);
s5 = sin(th5);
AA = -((c1*c2*c3+s1*s3)*c4+c1*s2*s4)*s5+(-c1*c2*s3+s1*c3)*c5;
BB = -((s1*c2*c3-c1*s3)*c4+s1*s2*s4)*s5+(-s1*c2*s3-c1*c3)*c5;
ss = (oz*AA - ox*BB)/(oz*nx - ox*nz );
cc = (nx*BB - nz*AA)/(oz*nx - ox*nz );
th7 = atan2(ss,cc);
jointPosition[0] = ASV_DEG(RAD2DEG(fmod(th1+M_PI_2, 2.*M_PI)));
jointPosition[1] = ASV_DEG(RAD2DEG(fmod(th2, 2.*M_PI)));
jointPosition[2] = ASV_DEG(RAD2DEG(fmod(th3, 2.*M_PI)));
jointPosition[3] = ASV_DEG(RAD2DEG(fmod(th4, 2.*M_PI)));
jointPosition[4] = ASV_DEG(RAD2DEG(fmod(th5, 2.*M_PI)));
jointPosition[5] = ASV_DEG(RAD2DEG(fmod(th6, 2.*M_PI)));
jointPosition[6] = ASV_DEG(RAD2DEG(fmod(th7, 2.*M_PI)));
return jointPosition;
}
TEST(TestSuite, oneDofRevoluteX)
{
const char* ffJointName = "base_footprint_joint";
jrl::dynamics::urdf::Parser parser;
CjrlHumanoidDynamicRobot* robot = parser.parse
(TEST_MODEL_DIRECTORY "/one_dof_revolute_x.urdf",
ffJointName);
CjrlJoint* root = robot->rootJoint ();
ASSERT_TRUE (root);
ASSERT_EQ (1, root->countChildJoints());
ASSERT_EQ (ffJointName, root->getName());
CjrlJoint* joint = root->childJoint (0);
ASSERT_TRUE (joint);
ASSERT_EQ ("joint1", joint->getName());
// Create robot configuration.
const int ndofs = 7;
vectorN q (ndofs);
vectorN dq (ndofs);
vectorN ddq (ndofs);
for (unsigned i = 0; i < ndofs; ++i)
q (i) = dq (i) = ddq (i) = 0.;
// Check configuration at zero.
std::cout << q << std::endl;
ASSERT_TRUE (robot->currentConfiguration(q));
ASSERT_TRUE (robot->currentVelocity(dq));
ASSERT_TRUE (robot->currentAcceleration(ddq));
ASSERT_TRUE (robot->computeForwardKinematics());
matrix4d jointPosition;
jointPosition.setIdentity ();
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_EQ(jointPosition (i, j),
root->currentTransformation () (i, j));
jointPosition.setIdentity ();
jointPosition (1, 3) = 1.;
// std::cout << "current value:" << std::endl;
// std::cout << joint->currentTransformation () << std::endl;
// std::cout << "expected value:" << std::endl;
// std::cout << jointPosition << std::endl;
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_NEAR (jointPosition (i, j),
joint->currentTransformation () (i, j),
1e-9);
// Check configuration at pi/2.
q (6) = M_PI / 2.;
std::cout << q << std::endl;
ASSERT_TRUE (robot->currentConfiguration(q));
ASSERT_TRUE (robot->currentVelocity(dq));
ASSERT_TRUE (robot->currentAcceleration(ddq));
ASSERT_TRUE (robot->computeForwardKinematics());
jointPosition.setIdentity ();
jointPosition (1, 1) = 0.;
jointPosition (1, 2) = -1.;
jointPosition (2, 1) = 1.;
jointPosition (2, 2) = 0.;
jointPosition (1, 3) = 1.;
std::cout << "current value:" << std::endl;
std::cout << joint->currentTransformation () << std::endl;
std::cout << "expected value:" << std::endl;
std::cout << jointPosition << std::endl;
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_NEAR (jointPosition (i, j),
joint->currentTransformation () (i, j),
1e-9);
}
