void CQDlgTranslations::_setCoord_userUnit(float newValueInUserUnit,bool orientation,int index)
{
int editMode=App::ct->objCont->getEditModeType();
C3DObject* object=App::ct->objCont->getLastSelection();
if ( (editMode==NO_EDIT_MODE)&&(object!=NULL) )
{
C7Vector tr;
if (coordMode==0)
tr=object->getCumulativeTransformationPart1();
else
tr=object->getLocalTransformationPart1();
tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
if (coordMode==0)
object->setLocalTransformation(object->getParentCumulativeTransformation().getInverse()*tr);
else
object->setLocalTransformation(tr);
}
if ( (editMode&PATH_EDIT_MODE)&&(App::ct->objCont->editModeBuffer.size()!=0)&&(App::ct->objCont->_editionPath!=NULL) )
{
CPathCont* pathCont=App::ct->objCont->_editionPath;
int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
CSimplePathPoint* pp=pathCont->getSimplePathPoint(ind);
CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
if ( (pp!=NULL)&&(path!=NULL) )
{
C7Vector tr(pp->getTransformation());
if (coordMode==0)
tr=path->getCumulativeTransformationPart1()*tr;
tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
if (coordMode==0)
pp->setTransformation(path->getCumulativeTransformation().getInverse()*tr,pathCont->getAttributes());
else
pp->setTransformation(tr,pathCont->getAttributes());
pathCont->actualizePath();
}
}
if ( (editMode&VERTEX_EDIT_MODE)&&(App::ct->objCont->editModeBuffer.size()!=0) )
{
int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
C3Vector v(App::ct->objCont->_editionVertices[3*ind+0],App::ct->objCont->_editionVertices[3*ind+1],App::ct->objCont->_editionVertices[3*ind+2]);
CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
if (shape!=NULL)
{
C7Vector tr;
tr.setIdentity();
tr.X=v;
if (coordMode==0)
tr=shape->getCumulativeTransformationPart1()*tr;
tr=_getNewTransf(tr,newValueInUserUnit,orientation,index);
if (coordMode==0)
tr=shape->getCumulativeTransformation().getInverse()*tr;
App::ct->objCont->_editionVertices[3*ind+0]=tr.X(0);
App::ct->objCont->_editionVertices[3*ind+1]=tr.X(1);
App::ct->objCont->_editionVertices[3*ind+2]=tr.X(2);
}
}
}
C7Vector CIKGraphJoint::getDownToTopTransformation()
{
C7Vector retVal;
retVal.setIdentity();
if (jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE)
retVal.Q=sphericalTransformation;
else if (jointType==IK_GRAPH_REVOLUTE_JOINT_TYPE)
retVal.Q.setAngleAndAxis(parameter,C3Vector(0.0f,0.0f,1.0f));
else if (jointType==IK_GRAPH_PRISMATIC_JOINT_TYPE)
retVal.X(2)=parameter;
return(retVal);
}
int simEmbSetObjectTransformation(int objectHandle,int relativeToObjectHandle,const float* position,const float* quaternion)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C3DObject* it=ct::objCont->getObject(objectHandle);
if (it==NULL)
return(-1);
if (relativeToObjectHandle==sim_handle_parent)
{
relativeToObjectHandle=-1;
C3DObject* parent=it->getParent();
if (parent!=NULL)
relativeToObjectHandle=parent->getID();
}
C3DObject* relObj=ct::objCont->getObject(relativeToObjectHandle);
if (relativeToObjectHandle!=-1)
{
if (relObj==NULL)
return(-1);
}
if (relativeToObjectHandle==-1)
{
C7Vector tr;
tr.Q(0)=quaternion[3];
tr.Q(1)=quaternion[0];
tr.Q(2)=quaternion[1];
tr.Q(3)=quaternion[2];
tr.X(0)=position[0];
tr.X(1)=position[1];
tr.X(2)=position[2];
ct::objCont->setAbsoluteConfiguration(it->getID(),tr,false);
}
else
{
C7Vector absTr(it->getCumulativeTransformationPart1());
C7Vector relTr(relObj->getCumulativeTransformationPart1()); // added ..Part1 on 2010/06/14
C7Vector x(relTr.getInverse()*absTr);
x.Q(0)=quaternion[3];
x.Q(1)=quaternion[0];
x.Q(2)=quaternion[1];
x.Q(3)=quaternion[2];
x.X(0)=position[0];
x.X(1)=position[1];
x.X(2)=position[2];
absTr=relTr*x;
ct::objCont->setAbsoluteConfiguration(it->getID(),absTr,false);
}
return(1);
}
C7Vector::C7Vector(float angle,const C3Vector& pos,const C3Vector& dir)
{ // Builds a rotation around dir at position pos of angle angle (in radians)
C7Vector shift1;
shift1.setIdentity();
shift1.X(0)=-pos(0);
shift1.X(1)=-pos(1);
shift1.X(2)=-pos(2);
C7Vector shift2;
shift2.setIdentity();
shift2.X=pos;
C7Vector rot;
rot.setIdentity();
rot.Q.setAngleAndAxis(angle,dir);
(*this)=shift2*rot*shift1;
}
void CikEl::checkIfWithinTolerance(bool& position,bool& orientation,bool useTempValues)
{
position=true;
orientation=true;
CDummy* targetObj=Ct::ct->objCont->getDummy(getTarget());
if (targetObj==NULL)
return; // The tooltip is not constrained!
CDummy* tooltipObj=Ct::ct->objCont->getDummy(tooltip);
C7Vector targetM(targetObj->getCumulativeTransformationPart1(useTempValues));
C7Vector tooltipM(tooltipObj->getCumulativeTransformationPart1(useTempValues));
// Since everything is relative to the base
C7Vector baseM;
baseM.setIdentity();
CDummy* baseObj=Ct::ct->objCont->getDummy(base);
if (baseObj!=NULL)
baseM=baseObj->getCumulativeTransformationPart1(useTempValues).getInverse();
baseObj=Ct::ct->objCont->getDummy(alternativeBaseForConstraints);
if (baseObj!=NULL)
baseM=baseObj->getCumulativeTransformationPart1(useTempValues).getInverse();
targetM=baseM*targetM;
tooltipM=baseM*tooltipM;
extIkReal err[2];
getError(targetM.getMatrix(),tooltipM.getMatrix(),err,(constraints&sim_ik_x_constraint)!=0,
(constraints&sim_ik_y_constraint)!=0,(constraints&sim_ik_z_constraint)!=0,
(constraints&sim_ik_alpha_beta_constraint)!=0,(constraints&sim_ik_gamma_constraint)!=0);
if (constraints&(sim_ik_x_constraint|sim_ik_y_constraint|sim_ik_z_constraint))
{
if (minLinearPrecision<err[0])
position=false;
}
if (constraints&(sim_ik_alpha_beta_constraint|sim_ik_gamma_constraint))
{
if (minAngularPrecision<err[1])
orientation=false;
}
}
int simEmbGetObjectTransformation(int objectHandle,int relativeToObjectHandle,float* position,float* quaternion)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C3DObject* it=ct::objCont->getObject(objectHandle);
if (it==NULL)
return(-1);
if (relativeToObjectHandle==sim_handle_parent)
{
relativeToObjectHandle=-1;
C3DObject* parent=it->getParent();
if (parent!=NULL)
relativeToObjectHandle=parent->getID();
}
C3DObject* relObj=ct::objCont->getObject(relativeToObjectHandle);
if (relativeToObjectHandle!=-1)
{
if (relObj==NULL)
return(-1);
}
C7Vector tr;
if (relativeToObjectHandle==-1)
tr=it->getCumulativeTransformationPart1();
else
{
C7Vector relTr(relObj->getCumulativeTransformationPart1()); // added ..Part1 on 2010/06/14
tr=relTr.getInverse()*it->getCumulativeTransformationPart1(); // Corrected bug on 2011/01/22: was getLocalTransformationPart1 before!!!
}
quaternion[0]=tr.Q(1);
quaternion[1]=tr.Q(2);
quaternion[2]=tr.Q(3);
quaternion[3]=tr.Q(0);
position[0]=tr.X(0);
position[1]=tr.X(1);
position[2]=tr.X(2);
return(1);
}
int simEmbRotateAroundAxis(const float* positionIn,const float* quaternionIn,const float* axisVector,const float* axisPosition,float angle,float* positionOut,float* quaternionOut)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector m;
m.Q(0)=quaternionIn[3];
m.Q(1)=quaternionIn[0];
m.Q(2)=quaternionIn[1];
m.Q(3)=quaternionIn[2];
m.X(0)=positionIn[0];
m.X(1)=positionIn[1];
m.X(2)=positionIn[2];
C3Vector ax(axisVector);
C3Vector pos(axisPosition);
float alpha=-atan2(ax(1),ax(0));
float beta=atan2(-sqrt(ax(0)*ax(0)+ax(1)*ax(1)),ax(2));
m.X-=pos;
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,angle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
m.X+=pos;
quaternionOut[0]=m.Q(1);
quaternionOut[1]=m.Q(2);
quaternionOut[2]=m.Q(3);
quaternionOut[3]=m.Q(0);
positionOut[0]=m.X(0);
positionOut[1]=m.X(1);
positionOut[2]=m.X(2);
return(1);
}
int simEmbInvertTransformation(float* position,float* quaternion)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr;
tr.Q(0)=quaternion[3];
tr.Q(1)=quaternion[0];
tr.Q(2)=quaternion[1];
tr.Q(3)=quaternion[2];
tr.X(0)=position[0];
tr.X(1)=position[1];
tr.X(2)=position[2];
tr.inverse();
quaternion[0]=tr.Q(1);
quaternion[1]=tr.Q(2);
quaternion[2]=tr.Q(3);
quaternion[3]=tr.Q(0);
position[0]=tr.X(0);
position[1]=tr.X(1);
position[2]=tr.X(2);
return(1);
}
int simEmbMultTransformationWithVector(const float* position,const float* quaternion,float* vect)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr;
tr.Q(0)=quaternion[3];
tr.Q(1)=quaternion[0];
tr.Q(2)=quaternion[1];
tr.Q(3)=quaternion[2];
tr.X(0)=position[0];
tr.X(1)=position[1];
tr.X(2)=position[2];
C3Vector v1(vect);
C3Vector v2(tr*v1);
vect[0]=v2(0);
vect[1]=v2(1);
vect[2]=v2(2);
return(1);
}
void CQDlgTranslations::_copyTransf(const C7Vector& tr,C7Vector& trIt,bool orientation,int mask)
{
if (orientation)
trIt.Q=tr.Q;
else
{
if (mask&1)
trIt.X(0)=tr.X(0);
if (mask&2)
trIt.X(1)=tr.X(1);
if (mask&4)
trIt.X(2)=tr.X(2);
}
}
void CQDlgTranslations::_transform(C7Vector& tr,int t,bool self)
{ // t==0: rotation, t==1: translation, t==2: scaling
if (t==2)
{
tr.X(0)=tr.X(0)*scalingValues[0];
tr.X(1)=tr.X(1)*scalingValues[1];
tr.X(2)=tr.X(2)*scalingValues[2];
}
else
{
C7Vector m;
m.setIdentity();
if (t==0)
m.Q.setEulerAngles(rotAngles[0],rotAngles[1],rotAngles[2]);
if (t==1)
m.X.set(translationValues);
if (self)
tr=tr*m;
else
tr=m*tr;
}
}
int simEmbInterpolateTransformations(const float* position1,const float* quaternion1,const float* position2,const float* quaternion2,float interpolFactor,float* positionOut,float* quaternionOut)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr1;
tr1.Q(0)=quaternion1[3];
tr1.Q(1)=quaternion1[0];
tr1.Q(2)=quaternion1[1];
tr1.Q(3)=quaternion1[2];
tr1.X(0)=position1[0];
tr1.X(1)=position1[1];
tr1.X(2)=position1[2];
C7Vector tr2;
tr2.Q(0)=quaternion2[3];
tr2.Q(1)=quaternion2[0];
tr2.Q(2)=quaternion2[1];
tr2.Q(3)=quaternion2[2];
tr2.X(0)=position2[0];
tr2.X(1)=position2[1];
tr2.X(2)=position2[2];
C7Vector trOut;
trOut.buildInterpolation(tr1,tr2,interpolFactor);
quaternionOut[0]=trOut.Q(1);
quaternionOut[1]=trOut.Q(2);
quaternionOut[2]=trOut.Q(3);
quaternionOut[3]=trOut.Q(0);
positionOut[0]=trOut.X(0);
positionOut[1]=trOut.X(1);
positionOut[2]=trOut.X(2);
return(1);
}
void CikEl::prepareIkEquations(extIkReal interpolFact)
{ // Before calling this function, make sure that joint's temp. param. are initialized!
// Make also sure the tooltip is built on a joint before 'base' and that base
// is parent of 'tooltip'.
// interpolFact is the interpolation factor we use to compute the target pose:
// interpolPose=tooltipPose*(1-interpolFact)+targetPose*interpolFact
// We first take care of dummies linked to path objects in a "sliding" manner (not fixed but assigned to the path):
// Case 1. Target is the free sliding dummy:
CDummy* dummyObj=Ct::ct->objCont->getDummy(getTarget());
CDummy* tipObj=Ct::ct->objCont->getDummy(tooltip);
// We get the jacobian and the rowJointIDs:
rowJointIDs=new std::vector<int>;
rowJointStages=new std::vector<int>;
C4X4Matrix oldMatr;
CMatrix* Ja=CIkRoutine::getJacobian(this,oldMatr,rowJointIDs,rowJointStages);
// oldMatr now contains the cumulative transf. matr. of tooltip relative to base
C4X4Matrix oldMatrInv(oldMatr.getInverse());
int doF=Ja->cols;
int equationNumber=0;
C4X4Matrix dummyCumul;
C4X4Matrix m;
if (dummyObj!=NULL)
{
C3DObject* baseObj=Ct::ct->objCont->getObject(base);
C4X4Matrix baseCumul;
baseCumul.setIdentity();
if (baseObj!=NULL)
baseCumul=baseObj->getCumulativeTransformation(true).getMatrix();
baseObj=Ct::ct->objCont->getObject(alternativeBaseForConstraints);
if (baseObj!=NULL)
baseCumul=baseObj->getCumulativeTransformation(true).getMatrix();
baseCumul.inverse();
dummyCumul=dummyObj->getCumulativeTransformationPart1(true).getMatrix();
dummyCumul=baseCumul*dummyCumul; // target is relative to the base (or the alternative base)!
C7Vector tr;
tr.buildInterpolation(oldMatr.getTransformation(),dummyCumul.getTransformation(),interpolFact);
m=tr;
// We prepare matrix and errorVector and their respective sizes:
if (constraints&sim_ik_x_constraint)
equationNumber++;
if (constraints&sim_ik_y_constraint)
equationNumber++;
if (constraints&sim_ik_z_constraint)
equationNumber++;
if (constraints&sim_ik_alpha_beta_constraint)
equationNumber+=2;
if (constraints&sim_ik_gamma_constraint)
equationNumber++;
}
matrix=new CMatrix(equationNumber,doF);
matrix_correctJacobian=new CMatrix(equationNumber,doF);
errorVector=new CMatrix(equationNumber,1);
if (dummyObj!=NULL)
{
// We set up the position/orientation errorVector and the matrix:
int pos=0;
if (constraints&sim_ik_x_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(0,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(0,i);
}
(*errorVector)(pos,0)=(m.X(0)-oldMatr.X(0))*positionWeight;
pos++;
}
if (constraints&sim_ik_y_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(1,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(1,i);
}
(*errorVector)(pos,0)=(m.X(1)-oldMatr.X(1))*positionWeight;
pos++;
}
if (constraints&sim_ik_z_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(2,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(2,i);
}
(*errorVector)(pos,0)=(m.X(2)-oldMatr.X(2))*positionWeight;
pos++;
}
if ( (constraints&sim_ik_alpha_beta_constraint)&&(constraints&sim_ik_gamma_constraint) )
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(3,i);
(*matrix)(pos+1,i)=(*Ja)(4,i);
(*matrix)(pos+2,i)=(*Ja)(5,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+2,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+2,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR;
pos=pos+3;
}
else
{
if (constraints&sim_ik_alpha_beta_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(3,i);
(*matrix)(pos+1,i)=(*Ja)(4,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR;
pos=pos+2;
}
if (constraints&sim_ik_gamma_constraint)
{ // sim_gamma_constraint can't exist without sim_alpha_beta_constraint!
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(5,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR;
pos++;
}
}
}
delete Ja; // We delete the jacobian!
}
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);
}
void CQDlgTranslations::_applyTransformation(int t)
{ // t==0: rotation, t==1: translation, t==2: scaling
int editMode=App::ct->objCont->getEditModeType();
int objSelSize=App::ct->objCont->getSelSize();
int editObjSelSize=App::ct->objCont->editModeBuffer.size();
if ( (editMode==NO_EDIT_MODE)&&(objSelSize>0) )
{
for (int i=0;i<objSelSize;i++)
{
C3DObject* object=App::ct->objCont->getObject(App::ct->objCont->getSelID(i));
bool hasParentPresent=false;
if ((transfMode==0)&&(t!=2)) // scaling is different!
{ // We do a transformation relative to the world. If this object has a parent that also is selected, we don't process this object!
C3DObject* p=object->getParent();
while (p!=NULL)
{
for (int j=0;j<objSelSize;j++)
{
if (App::ct->objCont->getSelID(j)==p->getID())
{
hasParentPresent=true;
break;
}
}
if (hasParentPresent)
break;
p=p->getParent();
}
}
if (!hasParentPresent)
{
C7Vector tr;
if (transfMode==0)
tr=object->getCumulativeTransformationPart1();
else
tr=object->getLocalTransformationPart1();
_transform(tr,t,transfMode==2);
if (transfMode==0)
tr=object->getParentCumulativeTransformation().getInverse()*tr;
object->setLocalTransformation(tr);
}
}
}
if ( (editMode&PATH_EDIT_MODE)&&(editObjSelSize>0)&&(App::ct->objCont->_editionPath!=NULL) )
{
CPathCont* pathCont=App::ct->objCont->_editionPath;
CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
for (int i=0;i<editObjSelSize;i++)
{
CSimplePathPoint* pp=pathCont->getSimplePathPoint(App::ct->objCont->editModeBuffer[i]);
if ( (pp!=NULL)&&(path!=NULL) )
{
C7Vector tr(pp->getTransformation());
if (transfMode==0)
tr=path->getCumulativeTransformationPart1()*tr;
_transform(tr,t,transfMode==2);
if (transfMode==0)
tr=path->getCumulativeTransformationPart1().getInverse()*tr;
pp->setTransformation(tr,pathCont->getAttributes());
}
}
pathCont->actualizePath();
}
if ( (editMode&VERTEX_EDIT_MODE)&&(editObjSelSize>0) )
{
CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
if (shape!=NULL)
{
for (int i=0;i<editObjSelSize;i++)
{
C7Vector tr;
tr.setIdentity();
int ind=App::ct->objCont->editModeBuffer[i];
tr.X.set(&App::ct->objCont->_editionVertices[3*ind+0]);
if (transfMode==0)
tr=shape->getCumulativeTransformationPart1()*tr;
_transform(tr,t,transfMode==2);
if (transfMode==0)
tr=shape->getCumulativeTransformationPart1().getInverse()*tr;
App::ct->objCont->_editionVertices[3*ind+0]=tr.X(0);
App::ct->objCont->_editionVertices[3*ind+1]=tr.X(1);
App::ct->objCont->_editionVertices[3*ind+2]=tr.X(2);
}
}
}
}
CIKGraphNode* CGeometricConstraintSolverInt::createTree(CIKGraphObjCont& graphContainer,C3DObject* objectOnTree,std::vector<C3DObject*>& exploredObjs,std::vector<C3DObject*>& links,bool keepShapes,int& baseObjectID)
{ // Creates a tree of linked objects (linked dummies are not followed!)
// Return value is the base IKGraphObject of that tree
CIKGraphNode* toBeReturned=NULL;
while (objectOnTree->getParent()!=NULL)
objectOnTree=objectOnTree->getParent();
baseObjectID=objectOnTree->getID();
std::vector<C3DObject*> objectsToExplore;
std::vector<CIKGraphObject*> lastAdded;
objectsToExplore.push_back(objectOnTree);
lastAdded.push_back(NULL);
std::vector<CIKGraphObject*> noParent;
while (objectsToExplore.size()!=0)
{
C3DObject* object=objectsToExplore.back();
objectsToExplore.pop_back();
CIKGraphObject* lastAddedNode=lastAdded.back();
lastAdded.pop_back();
// 1. We have to insert this object (maybe)
int insert=-1;
CJoint* act=NULL;
CDummy* dum=NULL;
if ( (object->getObjectType()==sim_object_shape_type) )
insert=3;
if (object->getObjectType()==sim_object_joint_type)
{
act=(CJoint*)object;
if ( (act->getJointMode()==sim_jointmode_ik)||(act->getJointMode()==sim_jointmode_ikdependent) )
insert=0;
}
if (object->getObjectType()==sim_object_dummy_type)
{
dum=(CDummy*)object;
if (dum->getLinkedDummyID()!=-1)
{
if (dum->getLinkType()==sim_dummy_linktype_gcs_loop_closure)
insert=1;
if (dum->getLinkType()==sim_dummy_linktype_gcs_tip)
insert=2;
}
}
if (insert!=-1)
{
CIKGraphObject* justInsertedObject=NULL;
CIKGraphJoint* justInsertedJoint=NULL;
C7Vector transf(object->getCumulativeTransformationPart1());
if (insert==0)
{
if (act->getJointType()==sim_joint_revolute_subtype)
justInsertedJoint=graphContainer.insertRevoluteJointNode(transf,act->getPosition(),act->getPositionIntervalMin(),act->getPositionIntervalRange(),act->getScrewPitch(),act->getPositionIsCyclic(),act->getIKWeight()); // added getPositionIsCyclic on 2009/07/11
if (act->getJointType()==sim_joint_prismatic_subtype)
justInsertedJoint=graphContainer.insertPrismaticJointNode(transf,act->getPosition(),act->getPositionIntervalMin(),act->getPositionIntervalRange(),act->getIKWeight());
if (act->getJointType()==sim_joint_spherical_subtype)
justInsertedJoint=graphContainer.insertBallJointNode(transf,act->getSphericalTransformation(),act->getPositionIntervalRange(),act->getIKWeight());
}
else if (insert==1)
{ // if we enter in this section, it is sure the dummies are linked and the link type is GCS_LOOP_CLOSURE
int data=dum->getID();
if (data>dum->getLinkedDummyID())
data=dum->getLinkedDummyID();
justInsertedObject=graphContainer.insertPassiveObjectNode(transf);
justInsertedObject->userData0=data;
links.push_back(App::ct->objCont->getDummy(dum->getLinkedDummyID()));
}
else if (insert==2)
{ // if we enter in this section, it is sure the dummies are linked and the link type is GCS_TIP
CDummy* targetD=App::ct->objCont->getDummy(dum->getLinkedDummyID());
justInsertedObject=graphContainer.insertTipObjectNode(transf,targetD->getCumulativeTransformation());
}
else if (insert==3)
{
justInsertedObject=graphContainer.insertPassiveObjectNode(transf);
//*********************************** IK Manipulation *********************************
if (object->getID()==App::ct->objCont->_ikManipulationObjectID)
{
C7Vector tipTransf;
C7Vector targetTransf;
tipTransf.setIdentity();
targetTransf.setIdentity();
tipTransf.X=App::ct->objCont->_ikManipulationStartPosRel;
tipTransf=object->getCumulativeTransformation()*tipTransf;
targetTransf.X=App::ct->objCont->_ikManipulationCurrentPosAbs;
CIKGraphObject* constr=graphContainer.insertTipObjectNode(tipTransf,targetTransf);
justInsertedObject->linkWithObject(constr);
}
//*************************************************************************************
}
if (insert!=0)
{
justInsertedObject->userData1=object->getID();
if (lastAddedNode!=NULL)
{
lastAddedNode->linkWithObject(justInsertedObject);
lastAddedNode=justInsertedObject;
}
else
{
lastAddedNode=justInsertedObject;
if (toBeReturned==NULL)
toBeReturned=justInsertedObject;
noParent.push_back(justInsertedObject);
}
}
else
{
justInsertedJoint->userData1=object->getID();
if (lastAddedNode!=NULL)
{
lastAddedNode->linkWithObject(justInsertedJoint->getDownIKGraphObject());
lastAddedNode=justInsertedJoint->getTopIKGraphObject();
}
else
{
lastAddedNode=justInsertedJoint->getTopIKGraphObject();
if (toBeReturned==NULL)
toBeReturned=justInsertedJoint;
noParent.push_back(justInsertedJoint->getDownIKGraphObject());
}
}
exploredObjs.push_back(object);
}
// 2. We prepare further exploration:
for (int i=0;i<int(object->childList.size());i++)
{
objectsToExplore.push_back(object->childList[i]);
lastAdded.push_back(lastAddedNode);
}
}
if (noParent.size()>1)
{ // We have to link those objects aginst each other (happens when the base object is not inserted)
for (int i=1;i<int(noParent.size());i++)
noParent[0]->linkWithObject(noParent[i]);
}
return(toBeReturned);
}
void C4X4Matrix::buildInterpolation(const C4X4Matrix& fromThis,const C4X4Matrix& toThat,float t)
{ // Builds the interpolation (based on t) from 'fromThis' to 'toThat'
C7Vector out;
out.buildInterpolation(fromThis.getTransformation(),toThat.getTransformation(),t);
(*this)=out;
}
void robot::createSensors()
{
std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors.");
printToConsole(txt.c_str());
for(size_t i = 0; i < vSensors.size() ; i++)
{
sensor *Sensor = vSensors.at(i);
if (Sensor->gazeboSpec)
printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems.");
else
{
if (Sensor->cameraSensorPresent)
{
int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0};
float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piValue/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f};
Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(Sensor->nSensor,std::string(name+"_camera").c_str());
}
int proxSensHandle=-1;
if (Sensor->proximitySensorPresent)
{ // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do
int intParams[8]={16,16,1,4,16,1,0,0};
float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piValue/180.0f,piValue/2.0f,0.0f,0.02f,0.0f,0.0f};
proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(proxSensHandle,std::string(Sensor->name+"_proximity").c_str());
}
// the doc doesn't state if a vision and proximity sensor can be declared at the same time...
if (proxSensHandle!=-1)
{
if (Sensor->nSensor!=-1)
{
Sensor->nSensorAux=proxSensHandle;
simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true);
}
else
Sensor->nSensor=proxSensHandle;
}
// Find the local configuration:
C7Vector sensorLocal;
sensorLocal.X.set(Sensor->origin_xyz);
sensorLocal.Q=getQuaternionFromRpy(Sensor->origin_rpy);
C4Vector rot(0.0f,0.0f,piValue); // the V-REP sensors are rotated by 180deg around the Z-axis
sensorLocal.Q=sensorLocal.Q*rot;
// We attach the sensor to a link:
C7Vector x;
x.setIdentity();
int parentLinkIndex=getLinkPosition(Sensor->parentLink);
if (parentLinkIndex!=-1)
{
int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent);
if (parentJointLinkIndex!=-1)
x=vJoints.at(parentJointLinkIndex)->jointBaseFrame;
}
C7Vector sensorGlobal(x*sensorLocal);
if (Sensor->nSensor!=-1)
{
simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data);
simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data);
}
if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1))
{
if (vLinks.at(parentLinkIndex)->visuals.size()!=0)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true);
if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true);
}
}
}
}
void robot::createJoints(bool hideJoints,bool positionCtrl)
{
std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints.");
printToConsole(txt.c_str());
//Set parents and childs for all the links
for(size_t i = 0; i < vJoints.size() ; i++)
{
vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name;
vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name;
}
//Create the joints
for(size_t i = 0; i < vJoints.size() ; i++)
{
//Move the joints to the positions specifieds by the urdf file
C7Vector tmp;
tmp.setIdentity();
tmp.X.set(vJoints.at(i)->origin_xyz);
tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy);
vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp;
//Set name jointParent to each joint
int nParentLink = getLinkPosition(vJoints.at(i)->parentLink);
vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent;
//Create the joint/forceSensor/dummy:
if (vJoints.at(i)->jointType==-1)
vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized
if (vJoints.at(i)->jointType==0)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==1)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==2)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==3)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==4)
{ // when joint type is "fixed"
int intParams[5]={1,4,4,0,0};
float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f};
vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL);
}
if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) )
{
float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit};
simSetJointInterval(vJoints.at(i)->nJoint,0,interval);
if (vJoints.at(i)->jointType==0)
{ // revolute
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular);
}
else
{ // prismatic
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear);
}
// We turn the position control on:
if (positionCtrl)
{
simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1);
simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1);
}
}
//Set the name:
setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str());
if (hideJoints)
simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10
}
//Set positions to joints from the 4x4matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data);
}
//Set joint parentship between them (thes parentship will be remove before adding the joints)
for(size_t i = 0; i < vJoints.size() ; i++)
{
int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint);
if ( parentJointIndex!= -1)
{
simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,nParentJoint,false);
}
}
//Delete all the partnership without moving the joints but after doing that update the transform matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix tmp;
simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data);
C3Vector euler;
simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data);
tmp.M.setEulerAngles(euler);
vJoints.at(i)->jointBaseFrame = tmp;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,-1,true);
}
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix jointAxisMatrix;
jointAxisMatrix.setIdentity();
C3Vector axis(vJoints.at(i)->axis);
C3Vector rotAxis;
float rotAngle=0.0f;
if (axis(2)<1.0f)
{
if (axis(2)<=-1.0f)
rotAngle=3.14159265359f;
else
rotAngle=acosf(axis(2));
rotAxis(0)=-axis(1);
rotAxis(1)=axis(0);
rotAxis(2)=0.0f;
rotAxis.normalize();
C7Vector m(jointAxisMatrix);
float alpha=-atan2(rotAxis(1),rotAxis(0));
float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2));
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,rotAngle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
jointAxisMatrix=m.getMatrix();
}
C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data);
}
}
bool CGeometricConstraintSolver::solve(CIKGraphObjCont& graphContainer,SGeomConstrSolverParam& parameters)
{
if (graphContainer.identifyElements()==0)
return(false); // Nothing to solve (no active joint in the mechanism)
graphContainer.putElementsInPlace();
// We create a branched tree, where each extremity has a tip dummy
// (which will later be constrained to its respective target dummy)
CIKGraphObject* baseIKGraphObject=graphContainer.getBaseObject();
CIKGraphNode* graphIterator=baseIKGraphObject;
CIKGraphNode* previousPosition=NULL;
CIKGraphNode* nextPosition=NULL;
C7Vector localTransformation;
localTransformation.setIdentity();
CIKObjCont ikObjs;
CIKJoint* lastJoint=NULL;
CIKJoint* treeHandle=NULL;
// Some precalculations of some fixed rotations:
C4X4Matrix tmpRot;
tmpRot.setIdentity();
tmpRot.M(0,0)=-1.0f;
tmpRot.M(2,2)=-1.0f;
C7Vector rotY180(tmpRot.getTransformation());
tmpRot.M.clear();
tmpRot.M(0,0)=1.0f;
tmpRot.M(2,1)=1.0f;
tmpRot.M(1,2)=-1.0f;
C7Vector rotX90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
tmpRot.M.clear();
tmpRot.M(2,0)=-1.0f;
tmpRot.M(1,1)=1.0f;
tmpRot.M(0,2)=1.0f;
C7Vector rotY90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
tmpRot.M.clear();
tmpRot.M(1,0)=1.0f;
tmpRot.M(0,1)=-1.0f;
tmpRot.M(2,2)=1.0f;
C7Vector rotZ90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f));
std::vector<CIKGraphNode*> graphObjectsToBeExplored;
graphObjectsToBeExplored.push_back(baseIKGraphObject);
std::vector<CIKJoint*> lastJoints;
lastJoints.push_back(NULL);
std::vector<CIKGraphNode*> previousPositions;
previousPositions.push_back(NULL);
std::vector<C7Vector> localTransformations;
localTransformations.push_back(localTransformation);
int explorationID=0;
while (graphObjectsToBeExplored.size()!=0)
{
graphIterator=graphObjectsToBeExplored.back();
graphObjectsToBeExplored.pop_back();
lastJoint=lastJoints.back();
lastJoints.pop_back();
previousPosition=previousPositions.back();
previousPositions.pop_back();
localTransformation=localTransformations.back();
localTransformations.pop_back();
bool doIt=(graphIterator->explorationID==-1);
bool goingDown=false;
bool closeComplexLoop=false;
while (doIt)
{
if (graphIterator->explorationID==-1)
graphIterator->explorationID=explorationID;
explorationID++;
C7Vector previousCT;
if (previousPosition!=NULL)
{
if (previousPosition->type==IK_GRAPH_JOINT_TYPE)
previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation;
else
previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation;
}
else
{
previousCT=baseIKGraphObject->cumulativeTransformation;
localTransformation=previousCT;
}
if (graphIterator->type==IK_GRAPH_JOINT_TYPE)
{ // Joint: we have to introduce a joint
CIKGraphJoint* graphJoint=(CIKGraphJoint*)graphIterator;
if (!graphJoint->disabled)
{
C7Vector sphTr;
sphTr.setIdentity();
sphTr.Q=graphJoint->sphericalTransformation;
CIKJoint* newIKJoint;
if (graphJoint->jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE)
{
int dataValueBase=10*graphJoint->nodeID;
CIKJoint* avatarParent;
if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
{ // From tip to base
C7Vector rel(localTransformation*rotY180);
newIKJoint=new CIKJoint(graphJoint,rel,false,false);
if (lastJoint==NULL)
{
treeHandle=newIKJoint;
lastJoint=treeHandle;
ikObjs.addRoot(lastJoint);
}
else
{
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
}
avatarParent=ikObjs.getJointWithData(dataValueBase+3);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+3;
rel=rotX90;
newIKJoint=new CIKJoint(graphJoint,rel,false,false);
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
avatarParent=ikObjs.getJointWithData(dataValueBase+2);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+2;
rel=rotY90;
newIKJoint=new CIKJoint(graphJoint,rel,false,false);
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
avatarParent=ikObjs.getJointWithData(dataValueBase+1);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+1;
rel=rotX90*rotZ90.getInverse()*sphTr.getInverse()*rotY180;
newIKJoint=new CIKJoint(graphJoint,rel,true,false);
lastJoint->topJoint=newIKJoint; // This is mainly needed by the joint-limitation part!
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
lastJoint->active=false; // Inactive for now (we can activate it later)
avatarParent=ikObjs.getJointWithData(dataValueBase+0);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+0;
localTransformation=rotY180;
}
else
{ // From base to tip
C7Vector rel(localTransformation);
newIKJoint=new CIKJoint(graphJoint,rel,false,true);
if (lastJoint==NULL)
{
treeHandle=newIKJoint;
lastJoint=treeHandle;
ikObjs.addRoot(lastJoint);
}
else
{
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
}
lastJoint->active=false; // Inactive for now (we can activate it later)
avatarParent=ikObjs.getJointWithData(dataValueBase+0);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+0;
rel=sphTr*rotY90;
newIKJoint=new CIKJoint(graphJoint,rel,false,true);
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
avatarParent=ikObjs.getJointWithData(dataValueBase+1);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+1;
rel=rotX90.getInverse();
newIKJoint=new CIKJoint(graphJoint,rel,false,true);
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
avatarParent=ikObjs.getJointWithData(dataValueBase+2);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+2;
rel=rotY90.getInverse()*rotZ90.getInverse();
newIKJoint=new CIKJoint(graphJoint,rel,true,true);
newIKJoint->topJoint=newIKJoint; // Top-joint is itself!
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
avatarParent=ikObjs.getJointWithData(dataValueBase+3);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValueBase+3;
localTransformation.setIdentity();
}
}
else
{
if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
{ // From tip to base
C7Vector rel(localTransformation*rotY180);
newIKJoint=new CIKJoint(graphJoint,rel,false,false);
localTransformation=rotY180;
}
else
{ // From base to tip
C7Vector rel(localTransformation);
newIKJoint=new CIKJoint(graphJoint,rel,false,false);
localTransformation.setIdentity();
}
if (lastJoint==NULL)
{
treeHandle=newIKJoint;
lastJoint=treeHandle;
ikObjs.addRoot(lastJoint);
}
else
{
ikObjs.addChild(lastJoint,newIKJoint);
lastJoint=newIKJoint;
}
int dataValue=10*graphJoint->nodeID+0;
CIKJoint* avatarParent=ikObjs.getJointWithData(dataValue);
if (avatarParent!=NULL) // This joint is used twice (going up and going down)
avatarParent->addAvatar(lastJoint);
lastJoint->data=dataValue;
}
}
else
{ // In case a graph-joint is disabled:
if (graphJoint->topObject==(CIKGraphObject*)previousPosition)
{ // From tip to base
localTransformation=localTransformation*graphJoint->getDownToTopTransformation().getInverse();
}
else
{ // From base to tip
localTransformation=localTransformation*graphJoint->getDownToTopTransformation();
}
}
}
else
{
CIKGraphObject* theObject=(CIKGraphObject*)graphIterator;
if (theObject->objectType==IK_GRAPH_LINK_OBJECT_TYPE)
{ // Link
if (previousPosition!=NULL)
{
if (theObject->linkPartner!=previousPosition)
localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation;
// If (theObject->linkPartner==previousPosition) then we don't do anything!
}
}
else
{ // Here we have a dummy we have to assign to a configuration or a passive object
// We treat all cases first as passive objects:
if (previousPosition!=NULL)
{
localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation;
if ( (theObject->objectType==IK_GRAPH_TIP_OBJECT_TYPE)&&(lastJoint!=NULL) )
{ // This is a valid dummy-tip!
CIKDummy* newIKDummy=new CIKDummy(localTransformation,theObject->targetCumulativeTransformation);
ikObjs.addChild(lastJoint,newIKDummy);
newIKDummy->constraints=(IK_X_CONSTRAINT|IK_Y_CONSTRAINT|IK_Z_CONSTRAINT);
newIKDummy->dampingFactor=1.0f;
newIKDummy->loopClosureDummy=false;
if (graphIterator->getConnectionNumber()==1)
break;
}
}
}
}
int unexploredSize=graphIterator->getNumberOfUnexplored();
if ( (unexploredSize==0)||goingDown||closeComplexLoop )
{
if ( (graphIterator->getConnectionNumber()==1)&&(!closeComplexLoop) )
break; // This is a rare case where we have an endpoint without a tip-dummy mobile-part
if (closeComplexLoop)
{
CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
ikObjs.addChild(lastJoint,tipDummy);
break;
}
nextPosition=graphIterator->getExploredWithSmallestExplorationID();
if ( (nextPosition->explorationID==0)&&(!goingDown) )
{ // The loop can now be closed (simple loop with each joint present at most once)
previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation;
localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation;
CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
ikObjs.addChild(lastJoint,tipDummy);
break;
}
if ( (nextPosition->explorationID==0)&&goingDown )
closeComplexLoop=true;
goingDown=true;
}
else if ((graphIterator->getNeighbourWithExplorationID(0)!=NULL)&&(!goingDown)&&(previousPosition->explorationID!=0))
{ // Here we have to close the loop too!
// We first put unexplored paths onto the stack:
for (int i=0;i<unexploredSize;i++)
{ // We throw unexplored nodes onto the exploration stack:
graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
lastJoints.push_back(lastJoint);
previousPositions.push_back(graphIterator);
localTransformations.push_back(localTransformation);
}
nextPosition=graphIterator->getExploredWithSmallestExplorationID();
previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation;
localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation;
CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation);
ikObjs.addChild(lastJoint,tipDummy);
break;
}
else
{
if (previousPosition==NULL)
{ // This is the start. We should always explore first two links which belong together
// or the 3 objects making up a joint!
nextPosition=NULL;
for (int i=0;i<unexploredSize;i++)
{
CIKGraphNode* nextPositionTmp=graphIterator->getUnexplored(i);
if ( (((CIKGraphObject*)graphIterator)->linkPartner==nextPositionTmp)||
(nextPositionTmp->type==IK_GRAPH_JOINT_TYPE) )
nextPosition=nextPositionTmp;
else
{
graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
lastJoints.push_back(lastJoint);
previousPositions.push_back(graphIterator);
localTransformations.push_back(localTransformation);
if (nextPosition==NULL)
nextPosition=graphIterator->getUnexplored(i);
}
}
}
else
{
nextPosition=graphIterator->getUnexplored(0);
for (int i=1;i<unexploredSize;i++)
{ // We throw unexplored nodes onto the exploration stack:
graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i));
lastJoints.push_back(lastJoint);
previousPositions.push_back(graphIterator);
localTransformations.push_back(localTransformation);
}
}
}
previousPosition=graphIterator;
graphIterator=nextPosition;
}
}
solveHierarchy(&ikObjs,parameters);
for (int i=0;i<int(ikObjs.allObjects.size());i++)
{
CIKObject* it=ikObjs.allObjects[i];
if (it->objectType==IK_JOINT_TYPE)
{
CIKJoint* theJoint=(CIKJoint*)it;
if (theJoint->avatarParent==NULL)
{
if (theJoint->spherical)
{
if (theJoint->topSpherical)
{
float a0=theJoint->parameter;
float a1=((CIKJoint*)theJoint->parent)->parameter;
float a2=((CIKJoint*)theJoint->parent->parent)->parameter;
float a3=((CIKJoint*)theJoint->parent->parent->parent)->parameter;
if (theJoint->sphericalUp)
{
theJoint->graphJoint->sphericalTransformation=C4Vector(a3,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a2,a1,a0));
}
else
{
theJoint->graphJoint->sphericalTransformation=C4Vector(a0,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a1,a2,a3));
}
}
}
else
theJoint->graphJoint->parameter=theJoint->parameter;
}
}
}
graphContainer.actualizeAllTransformations();
graphContainer.putElementsInPlace();
return(true);
}
void CIKGraphObjCont::actualizeTransformationsWithElementID(int elementID)
{
CIKGraphNode* it=NULL;
if (elementID==0)
it=getBaseObject();
else
{
for (int i=0;i<int(container.size());i++)
{
if ( (container[i]->elementID==elementID)&&(container[i]->type==IK_GRAPH_OBJECT_TYPE) )
{
it=container[i];
break;
}
}
}
C7Vector prevToCurrTr;
prevToCurrTr.setIdentity();
std::vector<C7Vector> prevToCurrTrs;
prevToCurrTrs.push_back(prevToCurrTr);
CIKGraphNode* prevNode=NULL;
std::vector<CIKGraphNode*> prevNodes;
prevNodes.push_back(prevNode);
CIKGraphNode* currentNode=it;
std::vector<CIKGraphNode*> currentNodes;
currentNodes.push_back(currentNode);
while (currentNodes.size()!=0)
{
prevToCurrTr=prevToCurrTrs.back();
prevToCurrTrs.pop_back();
prevNode=prevNodes.back();
prevNodes.pop_back();
currentNode=currentNodes.back();
currentNodes.pop_back();
C7Vector futurePrevOldTr;
if (currentNode->explorationID==-1)
{
if ( (prevNode!=NULL)&&(prevNode->type==IK_GRAPH_JOINT_TYPE) )
{ // We have : joint--->object
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
C7Vector nextCTM(((CIKGraphObject*)aNode)->cumulativeTransformation);
prevToCurrTrs.push_back(currentCTMI*nextCTM);
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
CIKGraphJoint* prevJoint=(CIKGraphJoint*)prevNode;
if (prevJoint->topObject==currentNode)
prevJoint->topObject->cumulativeTransformation=prevJoint->downObject->cumulativeTransformation*prevJoint->getDownToTopTransformation();
else
prevJoint->downObject->cumulativeTransformation=prevJoint->topObject->cumulativeTransformation*prevJoint->getDownToTopTransformation().getInverse();
}
else if (currentNode->type==IK_GRAPH_JOINT_TYPE)
{ // We have : object--->joint
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
prevToCurrTrs.push_back(prevToCurrTr); // The content doesn't matter here
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
}
else
{ // We have : object--->object or NULL--->object
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
C7Vector nextCTM;
if (aNode->type==IK_GRAPH_OBJECT_TYPE)
{
nextCTM=(((CIKGraphObject*)aNode)->cumulativeTransformation);
// In case aNode is a joint type, nextCTM can be anything (above)
}
if (aNode->elementID==elementID)
{ // We follow only same elementIDs!
prevToCurrTrs.push_back(currentCTMI*nextCTM);
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
}
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
if (prevNode!=NULL)
((CIKGraphObject*)currentNode)->cumulativeTransformation=((CIKGraphObject*)prevNode)->cumulativeTransformation*prevToCurrTr;
}
}
}
}
void CQDlgTranslations::_applyCoord(bool orientation,int mask)
{
int editMode=App::ct->objCont->getEditModeType();
C3DObject* object=App::ct->objCont->getLastSelection();
int objSelSize=App::ct->objCont->getSelSize();
int editObjSelSize=App::ct->objCont->editModeBuffer.size();
if ( (editMode==NO_EDIT_MODE)&&(object!=NULL)&&(objSelSize>1) )
{
C7Vector tr;
if (coordMode==0)
tr=object->getCumulativeTransformationPart1();
else
tr=object->getLocalTransformationPart1();
for (int i=0;i<objSelSize-1;i++)
{
C3DObject* it=App::ct->objCont->getObject(App::ct->objCont->getSelID(i));
C7Vector trIt;
if (coordMode==0)
trIt=it->getCumulativeTransformationPart1();
else
trIt=it->getLocalTransformationPart1();
_copyTransf(tr,trIt,orientation,mask);
if (coordMode==0)
it->setLocalTransformation(it->getParentCumulativeTransformation().getInverse()*trIt);
else
it->setLocalTransformation(trIt);
}
}
if ( (editMode&PATH_EDIT_MODE)&&(editObjSelSize>1)&&(App::ct->objCont->_editionPath!=NULL) )
{
CPathCont* pathCont=App::ct->objCont->_editionPath;
int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
CSimplePathPoint* pp=pathCont->getSimplePathPoint(ind);
CPath* path=App::ct->objCont->getPath(App::ct->objCont->getEditModeObjectID());
if ( (pp!=NULL)&&(path!=NULL) )
{
C7Vector tr(pp->getTransformation());
if (coordMode==0)
tr=path->getCumulativeTransformationPart1()*tr;
for (int i=0;i<editObjSelSize-1;i++)
{
CSimplePathPoint* ppIt=pathCont->getSimplePathPoint(App::ct->objCont->editModeBuffer[i]);
if (ppIt!=NULL)
{
C7Vector trIt(ppIt->getTransformation());
if (coordMode==0)
trIt=path->getCumulativeTransformationPart1()*trIt;
_copyTransf(tr,trIt,orientation,mask);
if (coordMode==0)
trIt=path->getCumulativeTransformationPart1().getInverse()*trIt;
ppIt->setTransformation(trIt,pathCont->getAttributes());
}
}
pathCont->actualizePath();
}
}
if ( (editMode&VERTEX_EDIT_MODE)&&(editObjSelSize>1) )
{
int ind=App::ct->objCont->editModeBuffer[App::ct->objCont->editModeBuffer.size()-1];
C3Vector v(App::ct->objCont->_editionVertices[3*ind+0],App::ct->objCont->_editionVertices[3*ind+1],App::ct->objCont->_editionVertices[3*ind+2]);
CShape* shape=App::ct->objCont->getShape(App::ct->objCont->getEditModeObjectID());
if (shape!=NULL)
{
C7Vector tr;
tr.setIdentity();
tr.X=v;
if (coordMode==0)
tr=shape->getCumulativeTransformationPart1()*tr;
for (int i=0;i<editObjSelSize-1;i++)
{
ind=App::ct->objCont->editModeBuffer[i];
C7Vector trIt;
trIt.setIdentity();
trIt.X.set(&App::ct->objCont->_editionVertices[3*ind+0]);
if (coordMode==0)
trIt=shape->getCumulativeTransformationPart1()*trIt;
_copyTransf(tr,trIt,orientation,mask);
if (coordMode==0)
trIt=shape->getCumulativeTransformationPart1().getInverse()*trIt;
App::ct->objCont->_editionVertices[3*ind+0]=trIt.X(0);
App::ct->objCont->_editionVertices[3*ind+1]=trIt.X(1);
App::ct->objCont->_editionVertices[3*ind+2]=trIt.X(2);
}
}
}
}
//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);
}
