Example #1
0
void IDIM::computeY(const MultiBody& mb, const MultiBodyConfig& mbc)
{
	const std::vector<Body>& bodies = mb.bodies();
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();

	Eigen::Matrix<double, 6, 10> bodyFPhi;
	for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
	{
		const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
		Eigen::Matrix<double, 6, 10> vb_i_Phi(IMPhi(vb_i));

		bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]);
		// bodyFPhi += vb_i x* IMPhi(vb_i)
		// is faster to convert each col in a ForceVecd
		// than using sva::vector6ToCrossDualMatrix
		for(int c = 0; c < 10; ++c)
		{
			bodyFPhi.col(c).noalias() +=
				(vb_i.crossDual(sva::ForceVecd(vb_i_Phi.col(c)))).vector();
		}

		int iDofPos = mb.jointPosInDof(i);

		Y_.block(iDofPos, i*10, joints[i].dof(), 10).noalias() =
			mbc.motionSubspace[i].transpose()*bodyFPhi;

		int j = i;
		while(pred[j] != -1)
		{
			const sva::PTransformd& X_p_j = mbc.parentToSon[j];
			// bodyFPhi = X_p_j^T bodyFPhi
			// is faster to convert each col in a ForceVecd
			// than using X_p_j.inv().dualMatrix()
			for(int c = 0; c < 10; ++c)
			{
				bodyFPhi.col(c) =
					X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector();
			}
			j = pred[j];

			int jDofPos = mb.jointPosInDof(j);
			if(joints[j].dof() != 0)
			{
				Y_.block(jDofPos, i*10, joints[j].dof(), 10).noalias() =
					mbc.motionSubspace[j].transpose()*bodyFPhi;
			}
		}
	}
}
Example #2
0
void forwardKinematics(const MultiBody& mb, MultiBodyConfig& mbc)
{
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();
	const std::vector<int>& succ = mb.successors();
	const std::vector<sva::PTransformd>& Xt = mb.transforms();

	for(std::size_t i = 0; i < joints.size(); ++i)
	{
		mbc.jointConfig[i] = joints[i].pose(mbc.q[i]);
		mbc.parentToSon[i] = mbc.jointConfig[i]*Xt[i];

		if(pred[i] != -1)
			mbc.bodyPosW[succ[i]] = mbc.parentToSon[i]*mbc.bodyPosW[pred[i]];
		else
			mbc.bodyPosW[succ[i]] = mbc.parentToSon[i];
	}
}
Example #3
0
InverseKinematics::InverseKinematics(const MultiBody& mb, int ef_index):
	max_iterations_(ik::MAX_ITERATIONS),
	lambda_(ik::LAMBDA),
	threshold_(ik::THRESHOLD),
	almost_zero_(ik::ALMOST_ZERO),
	ef_index_(ef_index),
	jac_(mb, mb.body(ef_index).name()),
	svd_()
{
}
Example #4
0
void forwardAcceleration(const MultiBody& mb, MultiBodyConfig& mbc,
	const sva::MotionVecd& A_0)
{
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();
	const std::vector<int>& succ = mb.successors();

	for(std::size_t i = 0; i < joints.size(); ++i)
	{
		const sva::PTransformd& X_p_i = mbc.parentToSon[i];

		const sva::MotionVecd& vj_i = mbc.jointVelocity[i];
		sva::MotionVecd ai_tan = joints[i].tanAccel(mbc.alphaD[i]);

		const sva::MotionVecd& vb_i = mbc.bodyVelB[i];

		if(pred[i] != -1)
			mbc.bodyAccB[succ[i]] = X_p_i*mbc.bodyAccB[pred[i]] + ai_tan + vb_i.cross(vj_i);
		else
			mbc.bodyAccB[succ[i]] = X_p_i*A_0 + ai_tan + vb_i.cross(vj_i);
	}
}
Example #5
0
void forwardVelocity(const MultiBody& mb, MultiBodyConfig& mbc)
{
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();
	const std::vector<int>& succ = mb.successors();

	for(std::size_t i = 0; i < joints.size(); ++i)
	{
		const sva::PTransformd& X_p_i = mbc.parentToSon[i];

		mbc.jointVelocity[i] = joints[i].motion(mbc.alpha[i]);
		mbc.motionSubspace[i] = joints[i].motionSubspace();

		if(pred[i] != -1)
			mbc.bodyVelB[succ[i]] = X_p_i*mbc.bodyVelB[pred[i]] + mbc.jointVelocity[i];
		else
			mbc.bodyVelB[succ[i]] = mbc.jointVelocity[i];

		sva::PTransformd E_0_i(mbc.bodyPosW[succ[i]].rotation());
		mbc.bodyVelW[succ[i]] = E_0_i.invMul(mbc.bodyVelB[succ[i]]);
	}
}
Example #6
0
int createRobot(const std::string & fileName, const std::string & robotName, bool useFingers)
{
	std::cout  << std::endl << " ** Creating the model for "<< fileName <<" ** useFingers " << useFingers << std::endl;
	cleanupStaticData();

	DECLARE_IFSTREAM(file_wrl,         ("../data/"+robotName+"/VRMLmain.wrl").c_str());
	DECLARE_IFSTREAM(file_jointLimits, ("../data/"+robotName+"/jointLimits.dat").c_str());
	DECLARE_IFSTREAM(file_torqueLimits, ("../data/"+robotName+"/torqueLimits.dat").c_str());
	DECLARE_IFSTREAM(file_TagToBody,    ("../data/"+robotName+"/tag_to_bodyname.txt").c_str());

	// if the wrml file is not found, stop right there.
	if (!file_wrl)
		return 1;

	//parsing
	MultiBody * mb = new MultiBody(robotName);
	mb->isRomeo_ = (robotName.size() > 5 && robotName.substr(0,5) == "romeo");
	mb->parseWRL(file_wrl);
	if(file_jointLimits)
		mb->parseJointLimits(file_jointLimits);
	if(file_torqueLimits)
		mb->parseTorqueLimits(file_torqueLimits);
	if(file_TagToBody)
		mb->parseTags(file_TagToBody);

	loadFeetData(fileName, mb);

	// Finalize the parsing
	//update the outer - inner body
	int numJoints = mb->nbJoints();
	for (int i=0; i<numJoints; ++i)
	{
		bool valid = true;
		std::map<int,Joint*>::iterator it = mb->jointMap_.find(i);
		Joint* j = it->second;
		assert(j!=0x0  && "Null joint");

		// update the inner, outer body info with the ids
		std::map<std::string,int>::iterator itb = Body::bodyNameToId_.find(j->innerBodyName_);
		if ( itb != Body::bodyNameToId_.end() )
			j->innerBodyId_=itb->second;
		else
		{
			j->innerBodyId_= -1;
			std::cerr << "Unable to find inner body " << j->innerBodyName_ << " for the joint " << j->name_ << std::endl;
		}

		std::map<std::string,int>::iterator it2 = Body::bodyNameToId_.find(j->outerBodyName_);
		if ( it2 != Body::bodyNameToId_.end() )
			j->outerBodyId_=it2->second;
		else
		{
			j->outerBodyId_= -1;
			std::cerr << "Unable to find outer body " << j->outerBodyName_ << " for the joint " << j->name_ << std::endl;
		}




		// check existence of inner Body
		if (mb->bodyMap_.find(j->innerBodyId_) != mb->bodyMap_.end() )
		{
			j->innerBody_ = mb->bodyMap_[j->innerBodyId_];
			mb->bodyMap_[j->innerBodyId_]->outerJointList_.push_back(j);
		}
		else
			valid = false;

		// check existence of outer Body
		if (mb->bodyMap_.find(j->outerBodyId_) != mb->bodyMap_.end() )
			j->outerBody_ = mb->bodyMap_[j->outerBodyId_];
		else
			valid = false;

		// if the joint is not valid, remove it
		if ( valid == false )
		{
			std::cerr << "/!\\ The joint " << j->name_ << " cannot be considered as valid  "
					<< "(in: " << j->innerBodyId_ << ",out: " << j->outerBodyId_ <<	")" << std::endl;
			mb->jointMap_.erase(i);
		}
	}

	// What do we do with the fingers?
	std::map<int,Joint*>::iterator it;
	for(it = mb->jointMap_.begin(); it != mb->jointMap_.end(); )
	{
		// if the id is negative (unofficial joint, we add it at the end
		//  after the other joints.
		if (it->first < 0)
		{
			Joint * joint = it->second;
			int idInit = joint->id_;
			joint->id_ = Joint::jointGlobalId_ - idInit;

			Joint::jointNameToId_[joint->name_] = joint->id_;
			mb->jointMap_[joint->id_] = joint;
			++it;

			if (! useFingers)
				joint->type_ = "fixed";

			mb->jointMap_.erase(idInit);
		}
		else
		{
			++it;
		}
	}


	// write data for amelif.
	std::string amelifFolder (robotName+"_af/data/");
	writeAmelif(mb, amelifFolder, fileName, robotName);

	// write data for Humans.
	std::string mapleFolder = (robotName+"_maple/src/MapleCodeGeneration/");
	writeMaple(mb, mapleFolder, robotName, useFingers);

	// write data for the sot.
	string openHRPFolder(robotName+"_sot/data/");
	writeOpenHRP(mb, openHRPFolder, fileName, robotName, useFingers);

	// write data for the sot.
	string urdfFolder(robotName+"_urdf/"+robotName+"_description/");
	writeRos(mb, urdfFolder, fileName, robotName);

	delete mb;
	return 0;
}