CMatrix* CIkRoutine::getJacobian(CikEl* ikElement,C4X4Matrix& tooltipTransf,std::vector<int>* rowJointIDs,std::vector<int>* rowJointStages)
{ // rowJointIDs is NULL by default. If not null, it will contain the ids of the joints
// corresponding to the rows of the jacobian.
// Return value NULL means that is ikElement is either inactive, either invalid
// tooltipTransf is the cumulative transformation matrix of the tooltip,
// computed relative to the base!
// The temporary joint parameters need to be initialized before calling this function!
// We check if the ikElement's base is in the chain and that tooltip is valid!
CDummy* tooltip=ct::objCont->getDummy(ikElement->getTooltip());
if (tooltip==NULL)
{ // Should normally never happen!
ikElement->setActive(false);
return(NULL);
}
C3DObject* base=ct::objCont->getObject(ikElement->getBase());
if ( (base!=NULL)&&(!tooltip->isObjectParentedWith(base)) )
{ // This case can happen (when the base's parenting was changed for instance)
ikElement->setBase(-1);
ikElement->setActive(false);
return(NULL);
}
// We check the number of degrees of freedom and prepare the rowJointIDs vector:
C3DObject* iterat=tooltip;
int doF=0;
while (iterat!=base)
{
iterat=iterat->getParent();
if ( (iterat!=NULL)&&(iterat!=base) )
{
if (iterat->getObjectType()==sim_object_joint_type)
{
if ( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) )
{
int d=((CJoint*)iterat)->getDoFs();
for (int i=d-1;i>=0;i--)
{
if (rowJointIDs!=NULL)
{
rowJointIDs->push_back(iterat->getID());
rowJointStages->push_back(i);
}
}
doF+=d;
}
}
}
}
CMatrix* J=new CMatrix(6,(unsigned char)doF);
std::vector<C4X4FullMatrix*> jMatrices;
for (int i=0;i<(doF+1);i++)
{
C4X4FullMatrix* matr=new C4X4FullMatrix();
if (i==0)
(*matr).setIdentity();
else
(*matr).clear();
jMatrices.push_back(matr);
}
// Now we go from tip to base:
iterat=tooltip;
C4X4FullMatrix buff;
buff.setIdentity();
int positionCounter=0;
C4X4FullMatrix d0;
C4X4FullMatrix dp;
C4X4FullMatrix paramPart;
CJoint* lastJoint=NULL;
int indexCnt=-1;
int indexCntLast=-1;
while (iterat!=base)
{
C3DObject* nextIterat=iterat->getParent();
C7Vector local;
if (iterat->getObjectType()==sim_object_joint_type)
{
if ( (((CJoint*)iterat)->getJointMode()!=sim_jointmode_ik)&&(((CJoint*)iterat)->getJointMode()!=sim_jointmode_ikdependent) )
local=iterat->getLocalTransformation(true);
else
{
CJoint* it=(CJoint*)iterat;
if (it->getJointType()==sim_joint_spherical_subtype)
{
if (indexCnt==-1)
indexCnt=it->getDoFs()-1;
it->getLocalTransformationExPart1(local,indexCnt--,true);
if (indexCnt!=-1)
nextIterat=iterat; // We keep the same object! (but indexCnt has decreased)
}
else
local=iterat->getLocalTransformationPart1(true);
}
}
else
local=iterat->getLocalTransformation(true);
buff=C4X4FullMatrix(local.getMatrix())*buff;
iterat=nextIterat;
bool activeJoint=false;
if (iterat!=NULL) // Following lines recently changed!
{
if (iterat->getObjectType()==sim_object_joint_type)
activeJoint=( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) );
}
if ( (iterat==base)||activeJoint )
{ // If base is NULL then the second part is not evaluated (iterat->getObjectType())
if (positionCounter==0)
{ // Here we have the first part (from tooltip to first joint)
d0=buff;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
else
{ // Here we have a joint:
if (lastJoint->getJointType()==sim_joint_revolute_subtype)
{
buildDeltaZRotation(d0,dp,lastJoint->getScrewPitch());
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildZRotation(lastJoint->getPosition(true));
}
else if (lastJoint->getJointType()==sim_joint_prismatic_subtype)
{
buildDeltaZTranslation(d0,dp);
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildTranslation(0.0f,0.0f,lastJoint->getPosition(true));
}
else
{ // Spherical joint part!
buildDeltaZRotation(d0,dp,0.0f);
multiply(d0,dp,positionCounter,jMatrices);
if (indexCntLast==-1)
indexCntLast=lastJoint->getDoFs()-1;
paramPart.buildZRotation(lastJoint->getTempParameterEx(indexCntLast--));
}
d0=buff*paramPart;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
buff.setIdentity();
lastJoint=(CJoint*)iterat;
positionCounter++;
}
}
int alternativeBaseForConstraints=ikElement->getAlternativeBaseForConstraints();
if (alternativeBaseForConstraints!=-1)
{
CDummy* alb=ct::objCont->getDummy(alternativeBaseForConstraints);
if (alb!=NULL)
{ // We want everything relative to the alternativeBaseForConstraints dummy orientation!
C7Vector alternativeBase(alb->getCumulativeTransformationPart1(true));
C7Vector currentBase;
currentBase.setIdentity();
if (base!=NULL)
currentBase=base->getCumulativeTransformation(true); // could be a joint, we want also the joint intrinsic transformation part!
C4X4FullMatrix correction((alternativeBase.getInverse()*currentBase).getMatrix());
dp.clear();
multiply(correction,dp,0,jMatrices);
}
}
// The x-, y- and z-component:
for (int i=0;i<doF;i++)
{
(*J)(0,i)=(*jMatrices[1+i])(0,3);
(*J)(1,i)=(*jMatrices[1+i])(1,3);
(*J)(2,i)=(*jMatrices[1+i])(2,3);
}
// We divide all delta components (to avoid distorsions)...
for (int i=0;i<doF;i++)
(*jMatrices[1+i])/=IK_DIVISION_FACTOR;
// ...and add the cumulative transform to the delta-components:
for (int i=0;i<doF;i++)
(*jMatrices[1+i])+=(*jMatrices[0]);
// We also copy the cumulative transform to 'tooltipTransf':
tooltipTransf=(*jMatrices[0]);
// Now we extract the delta Euler components:
C4X4FullMatrix mainInverse(*jMatrices[0]);
mainInverse.invert();
C4X4FullMatrix tmp;
// Alpha-, Beta- and Gamma-components:
for (int i=0;i<doF;i++)
{
tmp=mainInverse*(*jMatrices[1+i]);
C3Vector euler(tmp.getEulerAngles());
(*J)(3,i)=euler(0);
(*J)(4,i)=euler(1);
(*J)(5,i)=euler(2);
}
// We free the memory allocated for each joint variable:
for (int i=0;i<int(jMatrices.size());i++)
delete jMatrices[i];
return(J);
}
//Write
void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg)
{
std::string txt("Creating link '"+name+"'...");
printToConsole(txt.c_str());
//visuals.clear();
// Visuals
for (int i=0; i<visuals.size(); i++) {
urdfElement &visual = visuals[i];
if(!visual.meshFilename.empty())
{
std::string fname(visual.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=visual.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found.");
else
visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (!visual.n)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
else
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
printToConsole(txt.c_str());
}
else
visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling);
}
else if (!isArrayEmpty(visual.sphere_size))
visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL);
else if (!isArrayEmpty(visual.cylinder_size))
visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL);
else if (!isArrayEmpty(visual.box_size))
visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL);
}
//collisions.clear();
//mass=0.1;
//collision
for (int i=0; i<collisions.size(); i++) {
urdfElement &collision = collisions[i];
if(!collision.meshFilename.empty())
{
std::string fname(collision.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=collision.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found");
else
collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (collision.n == -1)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
else
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
printToConsole(txt.c_str());
}
else
{
collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling);
if (createVisualIfNone&&(visuals.size()==0))
{ // We create a visual from the collision shape (before it gets morphed hereafter):
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,collision.n);
simCopyPasteSelectedObjects();
addVisual();
currentVisual().n = simGetObjectLastSelection();
}
int p;
int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014
float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f};
if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) )
{
int aux=1+4+8+16+64;
if (showConvexDecompositionDlg)
aux=1+2+8+16+64;
showConvexDecompositionDlg=false;
simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes!
}
simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia
simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object
}
}
else if (!isArrayEmpty(collision.sphere_size))
collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL);
else if (!isArrayEmpty(collision.cylinder_size))
collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL);
else if (!isArrayEmpty(collision.box_size))
collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL);
}
// Hack to draw COM in the collision layer
/*
addCollision();
currentCollision().xyz[0] = inertial_xyz[0];
currentCollision().xyz[1] = inertial_xyz[1];
currentCollision().xyz[0] = inertial_xyz[2];
currentCollision().rpy[0] = 1.5;
float dummySize[3]={0.01f,0.01f,0.01f};
currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
*/
// Grouping collisions shapes
nLinkCollision = groupShapes(collisions);
// Inertia
if (inertiaPresent)
{
C3Vector euler;
if (nLinkCollision==-1)
{ // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP:
float dummySize[3]={0.01f,0.01f,0.01f};
//nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable!
nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
}
C7Vector inertiaFrame;
inertiaFrame.X.set(inertial_xyz);
inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy);
//simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data);
//C7Vector collisionFrame;
//collisionFrame.X.set(collision_xyz);
//collisionFrame.Q=getQuaternionFromRpy(collision_rpy);
C7Vector collisionFrame;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
//C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix());
C4X4Matrix x(inertiaFrame.getMatrix());
float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)};
simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i);
//std::cout << "Mass: " << mass << std::endl;
}
else
{
if (nLinkCollision!=-1)
{
std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?");
printToConsole(txt.c_str());
}
}
if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1))
{ // We create a visual from the collision shape (meshes were handled earlier):
addVisual();
urdfElement &visual = currentVisual();
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,nLinkCollision);
simCopyPasteSelectedObjects();
visual.n=simGetObjectLastSelection();
simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual
simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual
}
// Set the respondable mask:
if (nLinkCollision!=-1)
simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy
// Grouping shapes
nLinkVisual = groupShapes(visuals);
// Set the names, visibility, etc.:
if (nLinkVisual!=-1)
{
setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str());
const float specularDiffuse[3]={0.3f,0.3f,0.3f};
if (nLinkCollision!=-1)
{ // if we have a collision object, we attach the visual object to it, then forget the visual object
C7Vector collisionFrame;
C3Vector euler;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
C7Vector visualFrame;
simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data);
simGetObjectOrientation(nLinkVisual,-1,euler.data);
visualFrame.Q.setEulerAngles(euler);
C7Vector x(collisionFrame.getInverse()*visualFrame);
simSetObjectPosition(nLinkVisual,-1,x.X.data);
simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data);
simSetObjectParent(nLinkVisual,nLinkCollision,0);
}
}
if (nLinkCollision!=-1)
{
setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str());
if (hideCollisionLinks)
simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9
}
}
CMatrix* CIKChain::getJacobian(CIKDummy* tooltip,C4X4Matrix& tooltipTransf,std::vector<CIKJoint*>& theRowJoints,int jointNbToExclude)
{ // theRowJoints will contain the IK-joint objects corresponding to the columns of the jacobian.
// tooltipTransf is the cumulative transformation of the tooltip (aux. return value)
// The temporary joint parameters need to be initialized before calling this function!
// We check the number of degrees of freedom and prepare the theRowJoints vector:
CIKObject* iterat=tooltip;
int doF=0;
while (iterat!=NULL)
{
iterat=iterat->parent;
if ( (iterat!=NULL)&&(iterat->objectType==IK_JOINT_TYPE) )
{
if (((CIKJoint*)iterat)->active)
{
theRowJoints.push_back((CIKJoint*)iterat);
doF++;
}
}
}
// Here we have to compensate for jointNbToExclude:
if (jointNbToExclude>0)
{
doF-=jointNbToExclude;
if (doF<1)
{ // Impossible to get a Jacobian in that case!
theRowJoints.clear();
return(NULL);
}
theRowJoints.erase(theRowJoints.end()-jointNbToExclude,theRowJoints.end());
}
CMatrix* J=new CMatrix(6,(BYTE)doF);
std::vector<C4X4FullMatrix*> jMatrices;
jMatrices.reserve(doF+1);
jMatrices.clear();
for (int i=0;i<(doF+1);i++)
{
C4X4FullMatrix* matr=new C4X4FullMatrix();
if (i==0)
(*matr).setIdentity();
else
(*matr).clear();
jMatrices.push_back(matr);
}
// Now we go from tip to base:
iterat=tooltip;
C4X4FullMatrix buff;
buff.setIdentity();
int positionCounter=0;
C4X4FullMatrix d0;
C4X4FullMatrix dp;
C4X4FullMatrix paramPart;
CIKJoint* lastJoint=NULL;
int jointCounter=0;
while (iterat!=NULL)
{
C7Vector local;
if ((jointCounter<doF)&&((CIKJoint*)iterat)->active )
local=iterat->getLocalTransformationPart1(true);
else
local=iterat->getLocalTransformation(true);
if ( (iterat!=NULL)&&(iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->active )
jointCounter++;
buff=C4X4FullMatrix(local.getMatrix())*buff;
iterat=iterat->parent;
if ( (iterat==NULL)||((iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->active&&(positionCounter<doF)) )
{
if (positionCounter==0)
{ // Here we have the first part (from tooltip to first joint)
d0=buff;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
else
{ // Here we have a joint:
if (lastJoint->revolute)
{
buildDeltaZRotation(d0,dp,lastJoint->screwPitch);
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildZRotation(lastJoint->tempParameter);
}
else
{
buildDeltaZTranslation(d0,dp);
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildTranslation(0.0f,0.0f,lastJoint->tempParameter);
}
d0=buff*paramPart;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
buff.setIdentity();
lastJoint=(CIKJoint*)iterat;
positionCounter++;
}
}
// The x-, y- and z-component:
for (int i=0;i<doF;i++)
{
(*J)(0,i)=(*jMatrices[1+i])(0,3);
(*J)(1,i)=(*jMatrices[1+i])(1,3);
(*J)(2,i)=(*jMatrices[1+i])(2,3);
}
// We divide all delta components (to avoid distorsions)...
for (int i=0;i<doF;i++)
(*jMatrices[1+i])/=IK_DIVISION_FACTOR;
// ...and add the cumulative transform to the delta-components:
for (int i=0;i<doF;i++)
(*jMatrices[1+i])+=(*jMatrices[0]);
// We also copy the cumulative transform to 'tooltipTransf':
tooltipTransf=(*jMatrices[0]);
// Now we extract the delta Euler components:
C4X4FullMatrix mainInverse(*jMatrices[0]);
mainInverse.invert();
C4X4FullMatrix tmp;
// Alpha-, Beta- and Gamma-components:
for (int i=0;i<doF;i++)
{
tmp=mainInverse*(*jMatrices[1+i]);
C3Vector euler(tmp.getEulerAngles());
(*J)(3,i)=euler(0);
(*J)(4,i)=euler(1);
(*J)(5,i)=euler(2);
}
// We free the memory allocated for each joint variable:
for (int i=0;i<int(jMatrices.size());i++)
delete jMatrices[i];
// Now we have to combine columns which are linked to the same joints:
for (int i=0;i<int(theRowJoints.size());i++)
{
CIKJoint* aJoint=theRowJoints[i];
if (aJoint!=NULL)
{
for (int j=i+1;j<int(theRowJoints.size());j++)
{
CIKJoint* bJoint=theRowJoints[j];
if (bJoint!=NULL)
{
if (aJoint==bJoint->avatarParent)
{
for (int k=0;k<J->rows;k++)
(*J)(k,i)+=(*J)(k,j);
theRowJoints[j]=NULL; // We remove that joint
break;
}
if (bJoint==aJoint->avatarParent)
{
for (int k=0;k<J->rows;k++)
(*J)(k,j)+=(*J)(k,i);
theRowJoints[i]=NULL; // We remove that joint
break;
}
}
}
}
}
// And now we create the new matrix and rowJoints:
int colNb=0;
std::vector<CIKJoint*> rowJointsCopy(theRowJoints);
theRowJoints.clear();
for (int i=0;i<int(rowJointsCopy.size());i++)
{
if (rowJointsCopy[i]!=NULL)
colNb++;
}
CMatrix* oldMatrix=J;
J=new CMatrix(oldMatrix->rows,colNb);
int horizPos=0;
for (int i=0;i<oldMatrix->cols;i++)
{
if (rowJointsCopy[i]!=NULL)
{
for (int j=0;j<oldMatrix->rows;j++)
(*J)(j,horizPos)=(*oldMatrix)(j,i);
theRowJoints.push_back(rowJointsCopy[i]);
horizPos++;
}
}
delete oldMatrix;
return(J);
}
