C++ C7Vector::getMatrix方法代码示例

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;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    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++;
//.........这里部分代码省略.........