TEST(rosToEigen, transform) {
Eigen::Isometry3d transform1 = toEigen(makeTransform(makeVector3(0, 1.5, 2), makeQuaternion(1, 0, 0, 0)));
Eigen::Isometry3d transform2 = toEigen(makeTransform(makeVector3(-1.5, -2.6, -3.7), makeQuaternion(0, 0, 0, 1)));
Eigen::Vector3d const & p1 = transform1.translation();
Eigen::Vector3d const & p2 = transform2.translation();
Eigen::Quaterniond const & q1 = Eigen::Quaterniond{transform1.rotation()};
Eigen::Quaterniond const & q2 = Eigen::Quaterniond{transform2.rotation()};
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
ASSERT_NEAR(1, q1.x(), 1e-5);
ASSERT_NEAR(0, q1.y(), 1e-5);
ASSERT_NEAR(0, q1.z(), 1e-5);
ASSERT_NEAR(0, q1.w(), 1e-5);
ASSERT_NEAR(0, q2.x(), 1e-5);
ASSERT_NEAR(0, q2.y(), 1e-5);
ASSERT_NEAR(0, q2.z(), 1e-5);
ASSERT_NEAR(1, q2.w(), 1e-5);
}
TEST(rosToEigen, pose) {
Eigen::Isometry3d pose1 = toEigen(makePose(makePoint(0, 1.5, 2), makeQuaternion(1, 0, 0, 0)));
Eigen::Isometry3d pose2 = toEigen(makePose(makePoint(-1.5, -2.6, -3.7), makeQuaternion(0, 0, 0, 1)));
Eigen::Vector3d const & p1 = pose1.translation();
Eigen::Vector3d const & p2 = pose2.translation();
Eigen::Quaterniond const & q1 = Eigen::Quaterniond{pose1.rotation()};
Eigen::Quaterniond const & q2 = Eigen::Quaterniond{pose2.rotation()};
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
ASSERT_NEAR(1, q1.x(), 1e-5);
ASSERT_NEAR(0, q1.y(), 1e-5);
ASSERT_NEAR(0, q1.z(), 1e-5);
ASSERT_NEAR(0, q1.w(), 1e-5);
ASSERT_NEAR(0, q2.x(), 1e-5);
ASSERT_NEAR(0, q2.y(), 1e-5);
ASSERT_NEAR(0, q2.z(), 1e-5);
ASSERT_NEAR(1, q2.w(), 1e-5);
}
SpatialMotionVector SpatialMotionVector::operator*(const double scalar) const
{
SpatialMotionVector scaledVec;
toEigen(scaledVec.linearVec3) = scalar*toEigen(this->linearVec3);
toEigen(scaledVec.angularVec3) = scalar*toEigen(this->angularVec3);
return scaledVec;
}
TEST(rosToEigen, point32) {
Eigen::Vector3f p1 = toEigen(makePoint32(0, 1.5, 2));
Eigen::Vector3f p2 = toEigen(makePoint32(-1.5, -2.6, -3.7));
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
}
TEST(rosToEigen, vector3) {
Eigen::Vector3d p1 = toEigen(makeVector3(0, 1.5, 2));
Eigen::Vector3d p2 = toEigen(makeVector3(-1.5, -2.6, -3.7));
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
}
double ConvexHullProjectionConstraint::computeMargin(const Vector2& posIn2D)
{
bool isInside = true;
// First check if the point is inside the convex hull or not
iDynTree::VectorDynSize Ax(A.rows());
toEigen(Ax) = toEigen(A)*toEigen(posIn2D);
// If even one of the constraint is violated, then the point is outside the convex hull
for (int i=0; i < Ax.size(); i++)
{
if (!(Ax(i) <= b(i)))
{
isInside = false;
break;
}
}
// Then, compute the distance between each segment of the convex hull and the point :
// the minimum one is the distance of the point from the convex hull
// We start from the last segment that do not follow the segment[i],segment[i+1] structure
double distanceWithoutSign = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(projectedConvexHull.getNrOfVertices()-1),
projectedConvexHull(0));
// Find the minimum distance
for (int i=0; i < projectedConvexHull.getNrOfVertices()-1; i++)
{
double candidateDistance = distanceBetweenPointAndSegment(posIn2D,
projectedConvexHull(i),
projectedConvexHull(i+1));
if (candidateDistance < distanceWithoutSign)
{
distanceWithoutSign = candidateDistance;
}
}
double margin;
// If the point is inside the convex hull, we return the distance, otherwise the negated distance
if (isInside)
{
margin = distanceWithoutSign;
}
else
{
margin = -distanceWithoutSign;
}
return margin;
}
Matrix6x6 SpatialMotionVector::asCrossProductMatrixWrench() const
{
Matrix6x6 res;
Eigen::Map< Eigen::Matrix<double,6,6,Eigen::RowMajor> > retEigen(res.data());
retEigen.block<3,3>(0,0) = skew(toEigen(this->angularVec3));
retEigen.block<3,3>(0,3).setZero();
retEigen.block<3,3>(3,0) = skew(toEigen(this->linearVec3));
retEigen.block<3,3>(3,3) = skew(toEigen(this->angularVec3));
return res;
}
TEST(rosToEigen, quaternion) {
Eigen::Quaterniond q1 = toEigen(makeQuaternion(0, 1.5, 2, 3.6));
Eigen::Quaterniond q2 = toEigen(makeQuaternion(-1.5, -2.6, -3.7, 4.9));
ASSERT_NEAR(0.0, q1.x(), 1e-5);
ASSERT_NEAR(1.5, q1.y(), 1e-5);
ASSERT_NEAR(2.0, q1.z(), 1e-5);
ASSERT_NEAR(3.6, q1.w(), 1e-5);
ASSERT_NEAR(-1.5, q2.x(), 1e-5);
ASSERT_NEAR(-2.6, q2.y(), 1e-5);
ASSERT_NEAR(-3.7, q2.z(), 1e-5);
ASSERT_NEAR(4.9, q2.w(), 1e-5);
}
double distanceBetweenPointAndSegment(const Vector2& point,
const Vector2& segmentFirstPoint,
const Vector2& segmentLastPoint)
{
double segmentLengthSquared = (toEigen(segmentLastPoint)-toEigen(segmentFirstPoint)).squaredNorm();
// First case: the segment is actually a point
if (segmentLengthSquared == 0.0)
{
return distanceBetweentTwoPoints(point, segmentFirstPoint);
}
// The segment can be written as the set of segment points sp that can be written as:
// sp = segmentFirstPoint + t*( segmentLastPoint - segmentFirstPoint )
// for t \in [0, 1]
// To find the segment point closest to the point, we can just find the t of the projection
// of the point in the segment, and clamp it to [0, 1]
double tProjection = ((toEigen(point)-toEigen(segmentFirstPoint)).dot(toEigen(segmentLastPoint)-toEigen(segmentFirstPoint)))/segmentLengthSquared;
double tClosestPoint = std::max(0.0, std::min(1.0, tProjection));
Vector2 closestPoint;
toEigen(closestPoint) = toEigen(segmentFirstPoint) + tClosestPoint*(toEigen(segmentLastPoint)-toEigen(segmentFirstPoint));
return distanceBetweentTwoPoints(point, closestPoint);
}
bool Direction::isPerpendicular(const Direction& otherDirection, double tolerance) const
{
assert(tolerance > 0);
double fabsDotProduct = fabs(toEigen(*this).dot(toEigen(otherDirection)));
if( fabsDotProduct > tolerance )
{
return false;
}
else
{
return true;
}
}
std::vector< Eigen::Matrix4d > PhysicsEngine::GetVehiclePoses(
Vehicle_Entity* Vehicle ){
std::vector<Eigen::Matrix4d> VehiclePoses;
btTransform VehiclePose;
VehiclePose.setIdentity();
VehiclePose = Vehicle->m_pVehicle->getChassisWorldTransform();
VehiclePoses.push_back(toEigen(VehiclePose));
for( int i = 0; i<Vehicle->m_pVehicle->getNumWheels(); i++){
Vehicle->m_pVehicle->updateWheelTransform(i,false);
btTransform WheelPose;
WheelPose.setIdentity();
WheelPose = Vehicle->m_pVehicle->getWheelTransformWS(i);
VehiclePoses.push_back(toEigen(WheelPose));
}
return VehiclePoses;
}
void ConvexHullProjectionConstraint::buildConstraintMatrix()
{
// The rows of the A matrix and of the b vector depends on the number of vertices in the convex hull
A.resize(projectedConvexHull.getNrOfVertices(),2);
b.resize(projectedConvexHull.getNrOfVertices());
// We assume that the vertices in projectedConvexHull are expressed in counter-clockwise order: this
// is ensured by the structure of the Monotone Chain algorithm
// Iterate on all line segments one by one
for (size_t i=0; i < projectedConvexHull.getNrOfVertices(); i++)
{
Vector2 p0 = projectedConvexHull(i);
Vector2 p1;
if ( i != projectedConvexHull.getNrOfVertices()-1 )
{
p1 = projectedConvexHull(i+1);
}
else
{
// if we are using the last vertix, we build the line connecting it with the first vertex
p1 = projectedConvexHull(0);
}
toEigen(A).block<1,2>(i,0) = Eigen::Vector2d(p1(1)-p0(1),p0(0)-p1(0));
b(i) = p0(0)*p1(1) - p1(0)*p0(1);
}
return;
}
/**
* @function createDartNode
*/
dynamics::BodyNode* DartLoader::createDartNode(const urdf::Link* _lk, std::string _rootToSkelPath) {
dynamics::BodyNode* node = new dynamics::BodyNode(_lk->name);
// Mesh Loading
//FIXME: Shouldn't mass and inertia default to 0?
double mass = 0.1;
Eigen::Matrix3d inertia = Eigen::MatrixXd::Identity(3,3);
inertia *= 0.1;
// Load Inertial information
if(_lk->inertial) {
node->setLocalCOM(toEigen(_lk->inertial->origin.position));
node->setMass(_lk->inertial->mass);
node->setInertia(_lk->inertial->ixx, _lk->inertial->iyy, _lk->inertial->izz,
_lk->inertial->ixy, _lk->inertial->ixz, _lk->inertial->iyz);
}
// Set visual information
for(unsigned int i = 0; i < _lk->visual_array.size(); i++) {
if(dynamics::Shape* shape = createShape(_lk->visual_array[i].get(), _rootToSkelPath)) {
node->addVisualizationShape(shape);
}
}
// Set collision information
for(unsigned int i = 0; i < _lk->collision_array.size(); i++) {
if(dynamics::Shape* shape = createShape(_lk->collision_array[i].get(), _rootToSkelPath)) {
node->addCollisionShape(shape);
}
}
return node;
}
Eigen::VectorXd toEigen(const KDL::Wrench & f, const KDL::JntArray & tau)
{
VectorXd ret(6+tau.rows());
ret.segment(0,6) = toEigen(f);
for(int i=0; i < (int)tau.rows(); i++ ) { ret(6+i) = tau(i); }
return ret;
}
Eigen::VectorXd toEigen(const KDL::Twist & v, const KDL::JntArray & dq)
{
VectorXd ret(6+dq.rows());
ret.segment(0,6) = toEigen(v);
for(int i=0; i < (int)dq.rows(); i++ ) { ret(6+i) = dq(i); }
return ret;
}
/**
* @function createDartJoint
*/
dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt)
{
dynamics::Joint* joint;
switch(_jt->type) {
case urdf::Joint::REVOLUTE:
joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
joint->setPositionLowerLimit(0, _jt->limits->lower);
joint->setPositionUpperLimit(0, _jt->limits->upper);
joint->setDampingCoefficient(0, _jt->dynamics->damping);
break;
case urdf::Joint::CONTINUOUS:
joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
joint->setDampingCoefficient(0, _jt->dynamics->damping);
break;
case urdf::Joint::PRISMATIC:
joint = new dynamics::PrismaticJoint(toEigen(_jt->axis));
joint->setPositionLowerLimit(0, _jt->limits->lower);
joint->setPositionUpperLimit(0, _jt->limits->upper);
joint->setDampingCoefficient(0, _jt->dynamics->damping);
break;
case urdf::Joint::FIXED:
joint = new dynamics::WeldJoint();
break;
case urdf::Joint::FLOATING:
joint = new dynamics::FreeJoint();
break;
case urdf::Joint::PLANAR:
std::cout << "Planar joint not supported." << std::endl;
assert(false);
return NULL;
default:
std::cout << "Unsupported joint type." << std::endl;
assert(false);
return NULL;
}
joint->setName(_jt->name);
joint->setTransformFromParentBodyNode(toEigen(_jt->parent_to_joint_origin_transform));
joint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
if(joint->getNumDofs() == 1 && _jt->limits) {
joint->setVelocityLowerLimit(0, -_jt->limits->velocity);
joint->setVelocityUpperLimit(0, _jt->limits->velocity);
joint->setForceLowerLimit(0, -_jt->limits->effort);
joint->setForceUpperLimit(0, _jt->limits->effort);
}
return joint;
}
void ConvexHullProjectionConstraint::setProjectionAlongDirection(Vector3 direction)
{
Vector3 xProjection, yProjection;
// define the projection for the x-component
xProjection.setVal(0, 1.0);
xProjection.setVal(1, 0.0);
xProjection.setVal(2, -direction.getVal(0) / direction.getVal(2));
// define the projection for the y-component
yProjection.setVal(0, 0.0);
yProjection.setVal(1, 1.0);
yProjection.setVal(2, -direction.getVal(1) / direction.getVal(2));
// fill the projection matrix
toEigen(Pdirection).block<1, 3>(0,0) = toEigen(xProjection);
toEigen(Pdirection).block<1, 3>(1,0) = toEigen(yProjection);
}
bool Direction::isParallel(const Direction& otherDirection, double tolerance) const
{
// The tolerance should be positive
assert(tolerance > 0);
// Compute the difference of the norm
Eigen::Vector3d diff = toEigen(*this)-toEigen(otherDirection);
double diffNorm = diff.norm();
// There are two possibilities for two directions to be parallel
// either they point in the same direction, or in opposite directions
if( ( diffNorm < tolerance ) ||
( fabs(diffNorm-2) < tolerance ) )
{
return true;
}
else
{
return false;
}
}
simulation::WorldPtr DartLoader::parseWorldString(
const std::string& _urdfString, const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
if(_urdfString.empty())
{
dtwarn << "[DartLoader::parseWorldString] A blank string cannot be "
<< "parsed into a World. Returning a nullptr\n";
return nullptr;
}
std::shared_ptr<urdf_parsing::World> worldInterface =
urdf_parsing::parseWorldURDF(_urdfString, _baseUri);
if(!worldInterface)
{
dtwarn << "[DartLoader::parseWorldString] Failed loading URDF.\n";
return nullptr;
}
simulation::WorldPtr world(new simulation::World);
for(size_t i = 0; i < worldInterface->models.size(); ++i)
{
const urdf_parsing::Entity& entity = worldInterface->models[i];
dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton(
entity.model.get(), entity.uri, resourceRetriever);
if(!skeleton)
{
dtwarn << "[DartLoader::parseWorldString] Robot " << worldInterface->models[i].model->getName()
<< " was not correctly parsed!\n";
continue;
}
// Initialize position and RPY
dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);
if (dynamic_cast<dynamics::FreeJoint*>(rootJoint))
rootJoint->setPositions(dynamics::FreeJoint::convertToPositions(transform));
else
rootJoint->setTransformFromParentBodyNode(transform);
world->addSkeleton(skeleton);
}
return world;
}
void dynamicsRegressorLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
// Store the base_world translational transform in world orientation
KDL::Frame world_base_X_world_world = KDL::Frame(-(X_b[traversal.getBaseLink()->getLinkIndex()].p));
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//Base dynamics
// The base dynamics is expressed with the orientation of the world but
// with respect to the base origin
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(world_base_X_world_world*X_b[i])*netWrenchRegressor_i;
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
#ifndef NDEBUG
//std::cerr << "Calculating regressor columns for link " << link->getName() << " and joint " << child_link->getAdjacentJoint(parent_link)->getName() << std::endl;
#endif
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(6+dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
#ifndef NDEBUG
//std::cout << "Getting parent link of link of index " << child_link->getName() << " " << child_link->getLinkIndex() << std::endl;
//std::cout << "Current base " << traversal.order[0]->getName() << " " << traversal.order[0]->getLinkIndex() << std::endl;
#endif
parent_link = traversal.getParentLink(child_link);
}
}
}
/**
* @function createDartNode
*/
bool DartLoader::createDartNodeProperties(
const urdf::Link* _lk,
dynamics::BodyNode::Properties &node,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
node.mName = _lk->name;
// Load Inertial information
if(_lk->inertial) {
urdf::Pose origin = _lk->inertial->origin;
node.mInertia.setLocalCOM(toEigen(origin.position));
node.mInertia.setMass(_lk->inertial->mass);
Eigen::Matrix3d J;
J << _lk->inertial->ixx, _lk->inertial->ixy, _lk->inertial->ixz,
_lk->inertial->ixy, _lk->inertial->iyy, _lk->inertial->iyz,
_lk->inertial->ixz, _lk->inertial->iyz, _lk->inertial->izz;
Eigen::Matrix3d R(Eigen::Quaterniond(origin.rotation.w, origin.rotation.x,
origin.rotation.y, origin.rotation.z));
J = R * J * R.transpose();
node.mInertia.setMoment(J(0,0), J(1,1), J(2,2),
J(0,1), J(0,2), J(1,2));
}
// Set visual information
for(size_t i = 0; i < _lk->visual_array.size(); i++)
{
dynamics::ShapePtr shape = createShape(
_lk->visual_array[i].get(), _baseUri, _resourceRetriever);
if(shape)
node.mVizShapes.push_back(shape);
else
return false;
}
// Set collision information
for(size_t i = 0; i < _lk->collision_array.size(); i++) {
dynamics::ShapePtr shape = createShape(
_lk->collision_array[i].get(), _baseUri, _resourceRetriever);
if (shape)
node.mColShapes.push_back(shape);
else
return false;
}
return true;
}
bool FreeFloatingJacobianUsingLinkPos(const Model& model,
const Traversal& traversal,
const JointPosDoubleArray& jointPositions,
const LinkPositions& world_H_links,
const LinkIndex jacobianLinkIndex,
const Transform& jacobFrame_X_world,
const Transform& baseFrame_X_jacobBaseFrame,
MatrixDynSize& jacobian)
{
// We zero the jacobian
jacobian.zero();
// Compute base part
const Transform & world_H_base = world_H_links(traversal.getBaseLink()->getIndex());
toEigen(jacobian).block(0,0,6,6) = toEigen((jacobFrame_X_world*world_H_base*baseFrame_X_jacobBaseFrame).asAdjointTransform());
// Compute joint part
// We iterate from the link up in the traveral until we reach the base
LinkIndex visitedLinkIdx = jacobianLinkIndex;
while (visitedLinkIdx != traversal.getBaseLink()->getIndex())
{
LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex();
IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx);
size_t dofOffset = joint->getDOFsOffset();
for(int i=0; i < joint->getNrOfDOFs(); i++)
{
toEigen(jacobian).block(0,6+dofOffset+i,6,1) =
toEigen(jacobFrame_X_world*(world_H_links(visitedLinkIdx)*joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx)));
}
visitedLinkIdx = parentLinkIdx;
}
return true;
}
/**
* @function parseWorld
*/
simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {
std::string xmlString = readFileToString(_urdfFileName);
// Change path to a Unix-style path if given a Windows one
// Windows can handle Unix-style paths (apparently)
std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/');
std::string worldDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);
urdf::World* worldInterface = urdf::parseWorldURDF(xmlString, worldDirectory);
if(!worldInterface)
return NULL;
// Store paths from world to entities
parseWorldToEntityPaths(xmlString);
simulation::World* world = new simulation::World();
for(unsigned int i = 0; i < worldInterface->models.size(); ++i) {
std::string skeletonDirectory = worldDirectory + mWorld_To_Entity_Paths.find(worldInterface->models[i].model->getName())->second;
dynamics::Skeleton* skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get(), skeletonDirectory);
if(!skeleton) {
std::cout << "[ERROR] Robot " << worldInterface->models[i].model->getName() << " was not correctly parsed. World is not loaded. Exiting!" << std::endl;
return NULL;
}
// Initialize position and RPY
dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);
if(dynamic_cast<dynamics::FreeJoint*>(rootJoint)) {
Eigen::Vector6d coordinates;
coordinates << math::logMap(transform.linear()), transform.translation();
rootJoint->set_q(coordinates);
}
else {
rootJoint->setTransformFromParentBodyNode(transform);
}
world->addSkeleton(skeleton);
}
return world;
}
ArticulatedBodyInertia TransformDerivative::transform(const Transform& transform,
ArticulatedBodyInertia& other)
{
// Inefficient implementation for the time being, this should not be a bottleneck
Matrix6x6 oldABI = other.asMatrix();
Matrix6x6 a_X_b_wrench = transform.asAdjointTransformWrench();
Matrix6x6 a_dX_b_wrench = this->asAdjointTransformWrenchDerivative(transform);
Eigen::Matrix<double,6,6,Eigen::RowMajor> b_X_a = toEigen(a_X_b_wrench).transpose();
Eigen::Matrix<double,6,6,Eigen::RowMajor> b_dX_a = toEigen(a_dX_b_wrench).transpose();
Matrix6x6 newABI;
toEigen(newABI) = toEigen(a_dX_b_wrench)*toEigen(oldABI)*(b_X_a)
+toEigen(a_X_b_wrench)*toEigen(oldABI)*(b_dX_a);
return ArticulatedBodyInertia(newABI.data(),6,6);
}
dynamics::Shape* DartLoader::createShape(const VisualOrCollision* _vizOrCol, std::string _rootToSkelPath) {
dynamics::Shape* shape;
// Sphere
if(urdf::Sphere* sphere = dynamic_cast<urdf::Sphere*>(_vizOrCol->geometry.get())) {
shape = new dynamics::EllipsoidShape(2.0 * sphere->radius * Eigen::Vector3d::Ones());
}
// Box
else if(urdf::Box* box = dynamic_cast<urdf::Box*>(_vizOrCol->geometry.get())) {
shape = new dynamics::BoxShape(Eigen::Vector3d(box->dim.x, box->dim.y, box->dim.z));
}
// Cylinder
else if(urdf::Cylinder* cylinder = dynamic_cast<urdf::Cylinder*>(_vizOrCol->geometry.get())) {
shape = new dynamics::CylinderShape(cylinder->radius, cylinder->length);
}
// Mesh
else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get())) {
std::string fullPath = _rootToSkelPath + mesh->filename;
const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );
if(!model) {
std::cout<< "[add_Shape] [ERROR] Not loading model " << fullPath << " (NULL) \n";
shape = NULL;
}
else {
shape = new dynamics::MeshShape(Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model);
}
}
// Unknown geometry type
else {
std::cout << "[add_Shape] No MESH, BOX, CYLINDER OR SPHERE! Not loading this shape." << std::endl;
return NULL;
}
shape->setLocalTransform(toEigen(_vizOrCol->origin));
setMaterial(shape, _vizOrCol);
return shape;
}
SpatialForceVector TransformDerivative::transform(const Transform& transform,
SpatialForceVector& other)
{
SpatialForceVector ret;
Eigen::Map<const Eigen::Vector3d> p(transform.getPosition().data());
Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > R(transform.getRotation().data());
Eigen::Map<const Eigen::Vector3d> dp(this->posDerivative.data());
Eigen::Map<const Eigen::Matrix<double,3,3,Eigen::RowMajor> > dR(this->rotDerivative.data());
toEigen(ret.getLinearVec3()) = dR*toEigen(other.getLinearVec3());
toEigen(ret.getAngularVec3()) = dR*toEigen(other.getAngularVec3())
+ p.cross(toEigen(ret.getLinearVec3()))
+ dp.cross(R*toEigen(other.getLinearVec3()));
return ret;
}
void dynamicsRegressorFixedBaseLoop(const UndirectedTree & ,
const KDL::JntArray &q,
const Traversal & traversal,
const std::vector<Frame>& X_b,
const std::vector<Twist>& v,
const std::vector<Twist>& a,
Eigen::MatrixXd & dynamics_regressor)
{
dynamics_regressor.setZero();
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(int l =(int)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
LinkMap::const_iterator link = traversal.getOrderedLink(l);
int i = link->getLinkIndex();
//Each link affects the dynamics of the joints from itself to the base
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
//dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
LinkMap::const_iterator child_link = link;
LinkMap::const_iterator parent_link=traversal.getParentLink(link);
while( child_link != traversal.getOrderedLink(0) ) {
if( child_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
int dof_index = child_link->getAdjacentJoint(parent_link)->getDOFIndex();
int child_index = child_link->getLinkIndex();
Frame X_j_i = X_b[child_index].Inverse()*X_b[i];
dynamics_regressor.block(dof_index,10*i,1,10) =
toEigen(parent_link->S(child_link,q(dof_index))).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
child_link = parent_link;
parent_link = traversal.getParentLink(child_link);
}
}
}
int TreeInertialParameters::dynamicsRegressor( const JntArray &q,
const JntArray &q_dot,
const JntArray &q_dotdot,
const Twist& base_velocity,
const Twist& base_acceleration,
Eigen::MatrixXd & dynamics_regressor)
{
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || dynamics_regressor.cols()!=10*ns || dynamics_regressor.rows()!=(6+nj))
return -1;
unsigned int i;
unsigned int j=0;
//Kinematic propagation copied by RNE
for(unsigned int l = 0; l < recursion_order.size(); l++ ) {
unsigned int curr_index = recursion_order[l];
const Segment& seg = seg_vector[curr_index]->second.segment;
const Joint& jnt = seg.getJoint();
double q_,qdot_,qdotdot_;
if(jnt.getType() !=Joint::None){
int idx = link2joint[curr_index];
q_=q(idx);
qdot_=q_dot(idx);
qdotdot_=q_dotdot(idx);
}else
q_=qdot_=qdotdot_=0.0;
Frame& eX = X[curr_index];
Twist& eS = S[curr_index];
Twist& ev = v[curr_index];
Twist& ea = a[curr_index];
Wrench& ef = f[curr_index];
Frame& eX_b = X_b[curr_index];
assert(X.size() == ns);
//Calculate segment properties: X,S,vj,cj
X[curr_index] = seg.pose(q_);
//eX=seg.pose(q_);
//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=eX.M.Inverse(seg.twist(q_,qdot_));
eS=eX.M.Inverse(seg.twist(q_,1.0));
//We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
//calculate velocity and acceleration of the segment (in segment coordinates)
int parent_index = lambda[curr_index];
if( parent_index == -1 ) {
eX_b = eX;
} else {
eX_b = X_b[parent_index]*eX;
}
Twist parent_a, parent_v;
if( parent_index == -1 ) {
parent_a = base_acceleration;
parent_v = base_velocity;
} else {
parent_a = a[parent_index];
parent_v = v[parent_index];
}
ev=eX.Inverse(parent_v)+vj;
ea=eX.Inverse(parent_a)+eS*qdotdot_+ev*vj;
}
//end kinematic phase
dynamics_regressor.setZero();
//just for design, then the loop can be put together
Eigen::Matrix<double, 6, 10> netWrenchRegressor_i;
for(i=0;i<ns;i++) {
netWrenchRegressor_i = netWrenchRegressor(v[i],a[i]);
dynamics_regressor.block(0,(int)(10*i),6,10) = WrenchTransformationMatrix(X_b[i])*netWrenchRegressor_i;
Frame X_j_i;
for(j=0;j<ns;j++) {
X_j_i = X_b[j].Inverse()*X_b[i];
if( seg_vector[j]->second.segment.getJoint().getType() != Joint::None ) {
if( indicator_function[i][link2joint[j]] ) {
dynamics_regressor.block(6+link2joint[j],10*i,1,10) =
toEigen(S[j]).transpose()*WrenchTransformationMatrix(X_j_i)*netWrenchRegressor_i;
}
}
}
}
return 0;
}
int crba_momentum_jacobian_loop(const UndirectedTree & undirected_tree,
const Traversal & traversal,
const JntArray & q,
std::vector<RigidBodyInertia> & Ic,
MomentumJacobian & H,
RigidBodyInertia & InertiaCOM
)
{
#ifndef NDEBUG
if( undirected_tree.getNrOfLinks() != traversal.getNrOfVisitedLinks() ||
undirected_tree.getNrOfDOFs() != q.rows() ||
Ic.size() != undirected_tree.getNrOfLinks() ||
H.columns() != (undirected_tree.getNrOfDOFs() + 6) )
{
std::cerr << "crba_momentum_jacobian_loop: input data error" << std::endl;
return -1;
}
#endif
double q_;
Wrench F = Wrench::Zero();
//Sweep from root to leaf
for(int i=0; i<(int)traversal.getNrOfVisitedLinks(); i++)
{
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
//Collect RigidBodyInertia
Ic[link_index]=link_it->getInertia();
}
for(int i=(int)traversal.getNrOfVisitedLinks()-1; i >= 1; i-- ) {
int dof_id;
LinkMap::const_iterator link_it = traversal.getOrderedLink(i);
int link_index = link_it->getLinkIndex();
LinkMap::const_iterator parent_it = traversal.getParentLink(link_index);
int parent_index = parent_it->getLinkIndex();
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
dof_id = link_it->getAdjacentJoint(parent_it)->getDOFIndex();
q_ = q(dof_id);
} else {
q_ = 0.0;
dof_id = -1;
}
Ic[parent_index] = Ic[parent_index]+link_it->pose(parent_it,q_)*Ic[link_index];
if( link_it->getAdjacentJoint(parent_it)->getNrOfDOFs() == 1 ) {
KDL::Twist S_link_parent = parent_it->S(link_it,q_);
F = Ic[link_index]*S_link_parent;
if( traversal.getParentLink(link_it) != undirected_tree.getInvalidLinkIterator() ) {
double q__;
int dof_id_;
LinkMap::const_iterator predecessor_it = traversal.getParentLink(link_it);
LinkMap::const_iterator successor_it = link_it;
while(true) {
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
q__ = q( predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex());
} else {
q__ = 0.0;
}
F = successor_it->pose(predecessor_it,q__)*F;
successor_it = predecessor_it;
predecessor_it = traversal.getParentLink(predecessor_it);
if( predecessor_it == undirected_tree.getInvalidLinkIterator() ) {
break;
}
if( predecessor_it->getAdjacentJoint(successor_it)->getNrOfDOFs() == 1 ) {
dof_id_ = predecessor_it->getAdjacentJoint(successor_it)->getDOFIndex();
q__ = q(dof_id_);
} else {
q__ = 0.0;
dof_id_ = -1;
}
}
if( dof_id >= 0 ) {
H.data.block(0,6+dof_id,6,1) = toEigen(F);
}
//The first 6x6 submatrix of the Momentum Jacobian are simply the spatial inertia
//of all the structure expressed in the base reference frame
H.data.block(0,0,6,6) = toEigen(Ic[traversal.getBaseLink()->getLinkIndex()]);
}
}
}
//We have then to translate the reference point of the obtained jacobian to the com
//The Ic[traversal.order[0]->getLink(index)] contain the spatial inertial of all the tree
//expressed in link coordite frames
Vector com = Ic[traversal.getBaseLink()->getLinkIndex()].getCOG();
H.changeRefPoint(com);
InertiaCOM = Frame(com)*Ic[traversal.getBaseLink()->getLinkIndex()];
return 0;
}
int torqueRegressor::computeRegressor(const KDL::JntArray &q,
const KDL::JntArray &/*q_dot*/,
const KDL::JntArray &/*q_dotdot*/,
const std::vector<KDL::Frame> & X_dynamic_base,
const std::vector<KDL::Twist> & v,
const std::vector<KDL::Twist> & a,
const iDynTree::SensorsMeasurements & measured_wrenches,
const KDL::JntArray & measured_torques,
Eigen::MatrixXd & regressor_matrix_global_column_serialization,
Eigen::VectorXd & known_terms)
{
#ifndef NDEBUG
if( verbose ) std::cerr << "Called torqueRegressor::computeRegressor " << std::endl;
#endif
//const KDL::CoDyCo::UndirectedTree & = *p_undirected_tree;
//const KDL::CoDyCo::FTSensorList & ft_list = *p_ft_list;
if( regressor_local_parametrization.rows() != getNrOfOutputs() ) {
return -1;
}
/**
* \todo move this stuff in UndirectedTree
*
*/
JunctionMap::const_iterator torque_dof_it = p_undirected_tree->getJunction(torque_dof_index);
LinkMap::const_iterator parent_root_it;
if( torque_dof_it->getChildLink() == p_undirected_tree->getLink(subtree_root_link_id) ) {
parent_root_it = torque_dof_it->getParentLink();
} else {
parent_root_it = torque_dof_it->getChildLink();
}
assert(torque_dof_it->getJunctionIndex() < (int)p_undirected_tree->getNrOfDOFs());
KDL::Twist S = parent_root_it->S(p_undirected_tree->getLink(subtree_root_link_id),q(torque_dof_it->getJunctionIndex()));
//all other columns, beside the one relative to the inertial parameters of the links of the subtree, are zero
regressor_local_parametrization.setZero();
for(int i =0; i < (int)subtree_links_indices.size(); i++ ) {
int link_id = subtree_links_indices[i];
#ifndef NDEBUG
if( verbose ) std::cerr << "Adding to the torque regressor of joint " << torque_dof_it->getName() << " the regressor relative to link " << p_undirected_tree->getLink(link_id)->getName() << std::endl;
#endif
if( linkIndeces2regrCols[link_id] != -1 ) {
Eigen::Matrix<double,6,10> netWrenchRegressor_i = netWrenchRegressor(v[link_id],a[link_id]);
regressor_local_parametrization.block(0,(int)(10*linkIndeces2regrCols[link_id]),getNrOfOutputs(),10)
= toEigen(S).transpose()*WrenchTransformationMatrix(X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[link_id])*netWrenchRegressor_i;
}
}
#ifndef NDEBUG
if( consider_ft_offset ) {
if( verbose ) std::cerr << "considering ft offset" << std::endl;
} else {
if( verbose ) std::cerr << "not considering ft offset" << std::endl;
}
#endif
// if the ft offset is condidered, we have to set also the columns relative to the offset
// For each subgraph, we consider the measured wrenches as the one excerted from the rest of the
// tree to the considered subtree
// So the sign of the offset should be consistent with the other place where it is defined.
// In particular, the offset of a sensor is considered
// as an addictive on the measured wrench, so f_s = f_sr + f_o, where the frame of expression
// and the sign of f_s are defined in the SensorsTree structure
for(int leaf_id = 0; leaf_id < (int) subtree_leaf_links_indeces.size(); leaf_id++ ) {
if( consider_ft_offset ) {
int leaf_link_id = subtree_leaf_links_indeces[leaf_id];
// for now we are supporting just one six axis FT sensor for link
//std::vector< FTSensor> fts_link = ft_list.getFTSensorsOnLink(leaf_link_id);
//assert(fts_link.size()==1);
//int ft_id = fts_link[0].getID();
int ft_id = getFirstFTSensorOnLink(*p_sensors_tree,leaf_link_id);
assert( ft_id >= 0 );
iDynTree::SixAxisForceTorqueSensor * sens
= (iDynTree::SixAxisForceTorqueSensor *) p_sensors_tree->getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id);
assert( sens->isLinkAttachedToSensor(leaf_link_id) );
#ifndef NDEBUG
if( verbose ) std::cerr << "Adding to the torque regressor of joint " << torque_dof_it->getName()
<< " the columns relative to offset of ft sensor "
<< sens->getName() << std::endl;
#endif
/** \todo find a more robust way to get columns indeces relative to a given parameters */
assert(ft_id >= 0 && ft_id < 100);
assert(10*NrOfRealLinks_subtree+6*ft_id+5 < regressor_local_parametrization.cols());
double sign;
if( sens->getAppliedWrenchLink() == leaf_link_id ) {
sign = 1.0;
} else {
sign = -1.0;
}
iDynTree::Transform leaf_link_H_sensor;
bool ok = sens->getLinkSensorTransform(leaf_link_id,leaf_link_H_sensor);
assert(ok);
regressor_local_parametrization.block(0,(int)(10*NrOfRealLinks_subtree+6*ft_id),getNrOfOutputs(),6)
= sign*toEigen(S).transpose()*regressor_local_parametrization*WrenchTransformationMatrix(X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[leaf_link_id]*iDynTree::ToKDL(leaf_link_H_sensor));
}
}
//The known terms are simply all the measured wrenches acting on the subtree
// projected on the axis plus the measured torque
known_terms(0) = measured_torques(torque_dof_index);
#ifndef NDEBUG
//std::cerr << "computing kt " << std::endl;
//std::cerr << (ft_list).toString();
#endif
for(int leaf_id = 0; leaf_id < (int) subtree_leaf_links_indeces.size(); leaf_id++ ) {
int leaf_link_id = subtree_leaf_links_indeces[leaf_id];
int ft_id = getFirstFTSensorOnLink(*p_sensors_tree,leaf_link_id);
assert( ft_id >= 0 );
iDynTree::SixAxisForceTorqueSensor * sens
= (iDynTree::SixAxisForceTorqueSensor *) p_sensors_tree->getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id);
#ifndef NDEBUG
//std::cerr << "For leaf " << leaf_link_id << " found ft sensor " << ft_id << " that connects " << fts_link[0].getParent() << " and " << fts_link[0].getChild() << std::endl;
#endif
iDynTree::Wrench sensor_measured_wrench, link_measured_branch;
bool ok = measured_wrenches.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_id,sensor_measured_wrench);
ok = ok && sens->getWrenchAppliedOnLink(leaf_link_id,sensor_measured_wrench,link_measured_branch);
assert(ok);
known_terms(0) += dot(S,X_dynamic_base[subtree_root_link_id].Inverse()*X_dynamic_base[leaf_link_id]*iDynTree::ToKDL(link_measured_branch));
}
#ifndef NDEBUG
/*
std::cout << "Returning from computeRegressor (subtree):" << std::endl;
std::cout << "Regressor " << std::endl;
std::cout << regressor_matrix << std::endl;
std::cout << "known terms" << std::endl;
std::cout << known_terms << std::endl;
*/
#endif
convertLocalRegressorToGlobalRegressor(regressor_local_parametrization,regressor_matrix_global_column_serialization,this->localParametersIndexToOutputParametersIndex);
return 0;
}
