示例#1
0
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);
}
示例#2
0
//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
	}
}
示例#3
0
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);
}