/******************************************************************
* Solve Inverse Kinematics
* @deltaX : delta for end-effector position < R 3
* @deltaP : delta for palm direction < R 3
* @restQ : rest joint angles
* @deltaQ : output joint angles
******************************************************************/
void sKinematics::solveIKPalm(double delta_x[], int axis, double delta_dir[], double q_rest[], double delta_q[])
{
int i;
double in[6];
double *q, *q_null;
q = svector(dof);
q_null = svector(dof);
getJacobianPalm(axis, OJ );
for( i=0; i<3; i++ ){
in[i ] = delta_x[i];
in[i+3] = delta_dir[i];
}
//matsvdinv(OJ, 6, dof, OJi );
matsvdinvDLS(OJ, 6, dof, 0.01, OJi );
matmul_vec(OJi, dof, 6, in, delta_q );
// pseudo null space motion
getJoints(q);
matsub_c1(dof, q_rest, q, q_null );
for(i=0; i<dof; i++ ) q_null[i] *= lamda;
matnull(6, dof, OJ, OJi, N);
matmul_vec( N, dof, dof, q_null, q );
matadd_c1(dof, q, delta_q );
}
void UpperBodyPlanner::printOutJoints() {
std::vector<std::string> joints = getJoints();
if (joints.size() == 0) ROS_ERROR("Can not get joints.");
else {
for (int i = 0; i < joints.size(); i++) {
std::cout << "Joint number is: " << i << " joint name is: " << joints[i] << std::endl;
}
}
}
void UpperBodyPlanner::jointValue_moveTo(const Eigen::VectorXd& joint_value) {
if (joint_value.size() != 7) ROS_ERROR("Wrong size of joint value.");
else {
std::vector<std::string> joints = getJoints();
if (joints.size() == 0) ROS_ERROR("Can not get joints.");
else if (joints.size() < 7) ROS_WARN("Wrong joint size, please double check.");
else {
for (int i = 0; i < 7; i++) {
setJointValueTarget(joints[i], joint_value(i));
}
}
}
}
/******************************************************************
* Solve Inverse Kinematics
******************************************************************/
double sKinematics::solveIKPalmItr(double x_dest[], int axis, double dir_dest[], double q_rest[], double q_out[])
{
int i,j;
double *q_old, *q_curr, *q_delta, *q_null;
double x[3], dir[3];
double ox[6];
double error_old, error;
error_old = 1000.0; // set maximum error;
q_old = svector(dof);
q_curr = svector(dof);
q_delta= svector(dof);
q_null = svector(dof);
while(1){
getJacobianPalm(axis, OJ );
getEndPos(x);
getEndDirAxis(axis, dir);
getJoints(q_curr);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
//printf("%f \n", error );
// stop when the error reach to a local minimum
if( error < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( abs(error_old - error) < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( error > error_old ){
matcp_c1(dof, q_old, q_out );
return error_old;
}
matcp_c1(dof, q_curr, q_old );
// solve inverse kinematics
matsvdinvDLS(OJ, 6, dof, 0.01, OJi );
// set weight
for( i=0; i<dof; i++ ){
for( j=0; j<6; j++ ){
OJi[i][j] *= sDH[active_index[i]].weight;
}
}
matnull(6, dof, OJ, OJi, N );
matsub_c1(dof, q_rest, q_curr, q_null );
for(i=0; i<dof; i++ ) q_null[i] *= lamda;
matmul_vec(OJi, dof, 6, ox, q_delta );
matadd_c1(dof, q_delta, q_curr );
matmul_vec( N, dof, dof, q_null, q_delta );
matadd_c1(dof, q_delta, q_curr );
checkVelocityLimit(q_old, q_curr, deltaT);
setJoints(q_curr);
getEndPos(x);
getEndDirAxis(axis, dir);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
error_old = error;
}
}
double sKinematics::solveIKpointsItr(double x_dest[], double q_out[])
{
int i, j, k;
double *q_old, *q_curr, *q_delta, *q_init;
double x[3];
double *ox_delta;
double error_old, error;
int dim = 3*nPoints;
// set maximum error;
error_old = 100000.0;
q_old = svector(dof);
q_curr = svector(dof);
q_delta = svector(dof);
q_init = svector(dof);
ox_delta= svector(dim);
getJoints(q_old);
matcp_c1(dof, q_old, q_init );
while(1){
// get current joint
getJoints(q_curr);
// get jacobian and delta x
for( i=0; i<nPoints; i++){
getJacobianPos(ik_points_index[i], J);
for( j=0; j<3; j++ ){
for( k=0; k<dof; k++ ){
FJ[i*3+j][k] = J[j][k];
}
}
getEndPos(ik_points_index[i], x);
for( j=0; j<3; j++ ) {
ox_delta[i*3+j] = x_dest[i*3+j]-x[j];
}
}
// calculate error
error = 0;
for( i=0; i<dim; i++ ){
error += ox_delta[i]*ox_delta[i]*vweight[i];
}
error = sqrt(error);
//printf("%f \n", error );
// exit when the error reach a local minimum
if( error < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( abs(error_old - error) < IK_TOLERANCE*0.1 ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( error > error_old ){
matcp_c1(dof, q_old, q_out );
return error_old;
}
// solve IK
matsvdinvDLS(FJ, dim, dof, 0.5, FJi );
matmul_vec(FJi, dof, dim, ox_delta, q_delta );
matcp_c1(dof, q_curr, q_old ); // backup joint angles
matadd_c1(dof, q_delta, q_curr ); // set current joint
checkVelocityLimit(q_init, q_curr, deltaT);
setJoints(q_curr);
error_old = error;
}
return error;
}
void CX3DOpenHRPHumanoidNode::print(int indent)
{
FILE *fp = CX3DParser::getDebugLogFp();
int nMax = CX3DParser::getMaxPrintElemsForMFField();
bool bPartialPrint = false;
char *nodeName = getNodeName();
if (nodeName)
{
float x, y, z, rot;
MFNode *nodes;
int i, n;
CX3DParser::printIndent(indent);
fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));
CX3DParser::printIndent(indent+1);
getCenter()->getValue(x, y, z);
fprintf(fp, "center : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
getRotation()->getValue(x, y, z, rot);
fprintf(fp, "rotation : (%f %f %f)(%f)\n", x, y, z, rot);
CX3DParser::printIndent(indent+1);
getScale()->getValue(x, y, z);
fprintf(fp, "scale : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
getScaleOrientation()->getValue(x, y, z, rot);
fprintf(fp, "scaleOrientation : (%f %f %f)(%f)\n", x, y, z, rot);
CX3DParser::printIndent(indent+1);
getTranslation()->getValue(x, y, z);
fprintf(fp, "translation : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
fprintf(fp, "name (%s)\n", m_name.getValue());
CX3DParser::printIndent(indent+1);
n = m_info.count();
fprintf(fp, "info [%d]\n", n);
if ((nMax > 0) && (n > nMax)) { n = nMax; bPartialPrint = true; }
else { bPartialPrint = false; }
for (i=0; i<n; i++)
{
CX3DParser::printIndent(indent+2);
fprintf(fp, "%s\n", m_info.getValue(i));
}
if (bPartialPrint)
{
CX3DParser::printIndent(indent+2);
fprintf(fp, "...\n");
}
CX3DParser::printIndent(indent+1);
nodes = getJoints();
n = nodes->count();
fprintf(fp, "joints [%d]\n", n);
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
CX3DParser::printIndent(indent+1);
nodes = getSegments();
n = nodes->count();
fprintf(fp, "segments [%d]\n", n);
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
CX3DParser::printIndent(indent+1);
nodes = getSites();
n = nodes->count();
fprintf(fp, "sites [%d]\n", n);
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
CX3DParser::printIndent(indent+1);
nodes = getHumanoidBody();
n = nodes->count();
fprintf(fp, "humanoidBody [%d]\n", n);
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
CX3DParser::printIndent(indent+1);
fprintf(fp, "version (%s)\n", m_version.getValue());
CX3DParser::printIndent(indent+1);
nodes = getViewpoints();
n = nodes->count();
fprintf(fp, "viewpoints [%d]\n", n);
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
}
}
void SkeletonBlendedGeometry::calculatePositions(void)
{
if(getBaseGeometry() == NULL)
{
//Error
SWARNING << "SkeletonBlendedGeometry::calculatePositions(): Base Geometry is NULL." << std::endl;
return;
if(getPositions() == NULL)
{
//Error
SWARNING << "SkeletonBlendedGeometry::calculatePositions(): Positions is NULL." << std::endl;
return;
}
if(getBaseGeometry()->getPositions() == NULL)
{
//Error
SWARNING << "SkeletonBlendedGeometry::calculatePositions(): Base Geometry Postions is NULL." << std::endl;
return;
}
if(getMFPositionIndexes()->size() != getMFJoints()->size())
{
//Error
SWARNING << "SkeletonBlendedGeometry::calculatePositions(): Positions Indexes size is not the same as the affecting Joints size." << std::endl;
return;
}
}
if(getMFPositionIndexes()->size() != getMFBlendAmounts()->size())
{
//Error
SWARNING << "SkeletonBlendedGeometry::calculatePositions(): Positions Indexes size is not the same as the affecting blend amount size." << std::endl;
return;
}
Pnt3f CalculatedPoint;
Pnt3f PointTemp;
Vec3f CalculatedNormal;
//Set the values of all points to 0
for (int i(0); i < getPositions()->size(); ++i)
{
getPositions()->setValue(Pnt3f(0, 0, 0), i);
}
//Update the Positions and Normals
for(UInt32 i(0) ; i < getMFPositionIndexes()->size() ; ++i)
{
Matrix temp = getJoints(i)->getAbsoluteDifferenceTransformation();
temp.scale(getBlendAmounts(i));
getBaseGeometry()->getPositions()->getValue<Pnt3f>(PointTemp, getPositionIndexes(i));
temp.mult(PointTemp, CalculatedPoint);
//temp.mult(getBaseGeometry()->getNormals()->getValue(getPositionIndexes(i)), CalculatedNormal);
//Add
CalculatedPoint += PointTemp.subZero();
getPositions()->setValue<Pnt3f>(CalculatedPoint, getPositionIndexes(i));
}
for(UInt32 i = 0; i < _mfParents.size(); i++)
{
_mfParents[i]->invalidateVolume();
}
_volumeCache.setValid();
_volumeCache.setEmpty();
}
