예제 #1
0
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);
		}
	}
}
예제 #2
0
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);
}
예제 #3
0
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);
}
예제 #4
0
파일: 7Vector.cpp 프로젝트: ehsan1384/Vrep
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;
}
예제 #5
0
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;
	}
}
예제 #6
0
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);
}
예제 #7
0
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);
}
예제 #8
0
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);
}
예제 #9
0
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);
}
예제 #10
0
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);
	}
}
예제 #11
0
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;
	}
}
예제 #12
0
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);
}
예제 #13
0
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!
}
예제 #14
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);
}
예제 #15
0
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);
}
예제 #17
0
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;
}
예제 #18
0
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);
				}
			}
		}
}
예제 #19
0
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);
}
예제 #21
0
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;
			}
		}
	}
}
예제 #22
0
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);
			}
		}
	}
}
예제 #23
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
//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
	}
}
예제 #24
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);
}