void Line3D::jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Eigen::Isometry3d& x, const Line3D& l){
Jp.setZero();
Jl.setZero();
Matrix6d A=Matrix6d::Zero();
A.block<3,3>(0,0)=x.linear();
A.block<3,3>(0,3)= _skew(x.translation())*x.linear();
A.block<3,3>(3,3)=x.linear();
Vector6d v=(Vector6d)l;
Line3D lRemapped(v);
Matrix6d D = Matrix6d::Zero();
D.block<3,3>(0,0) = -_skew(l.d());
D.block<3,3>(0,3) = -2*_skew(l.w());
D.block<3,3>(3,3) = -2*_skew(l.d());
Jp.block<6,6>(0,0) = A*D;
Vector3d d = l.d();
Vector3d w = l.w();
double ln = d.norm();
double iln = 1./ln;
double iln3 = std::pow(iln,3);
Matrix6d Jll;
Jll <<
iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3,
0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3,
0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3,
0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3,
0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3,
0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3;
Jl.block<6,6>(0,0) = A*Jll;
Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z();
}
/**
* @function setRootTransform
* @brief Set q (SCREW) from pose <x,y,z,r,p,y>
*/
void InspectorTab::setRootTransform( dart::dynamics::Skeleton* robot,
const Eigen::Matrix<double, 6, 1>& pose ) {
dart::dynamics::Joint* joint = robot->getRootBodyNode()->getParentJoint();
if(dynamic_cast<dart::dynamics::FreeJoint*>(joint)) {
Matrix<double, 6, 1> q;
Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
transform.translation() = pose.head<3>();
transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
q = dart::math::logMap(transform);
joint->set_q( q );
}
else {
Eigen::Isometry3d transform;
transform.makeAffine();
transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
transform.translation() = pose.head<3>();
joint->setTransformFromParentBodyNode(transform);
}
for (int i = 0; i < robot->getNumBodyNodes(); ++i) {
robot->getBodyNode(i)->updateTransform();
}
}
//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
// Y-axis
Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();
// X-axis
Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
double mag = yAxis.dot(pelvisXAxis);
pelvisXAxis -= mag * yAxis;
xAxis = pelvisXAxis.normalized();
// Z-axis
Eigen::Vector3d zAxis = xAxis.cross(yAxis);
T.translation() = getCOM();
T.linear().col(0) = xAxis;
T.linear().col(1) = yAxis;
T.linear().col(2) = zAxis;
return T;
}
bool FLOATING_T::run(robot::robot_state_t& target, control_data_t* data)
{
// 1. Setup
Eigen::Isometry3d body;
target.get_body(body);
// 1. Variables
Eigen::Quaterniond save_rotation(data->joy_rotation);
Eigen::Vector3d save_position = data->joy_position;
Eigen::Quaterniond rotation(data->joy_rotation);
Eigen::Vector3d& position = data->joy_position;
// 2. Clamp to xyy
position[2] = 0;
rotation.y() = 0;
rotation.x() = 0;
body.linear() = body.linear() * rotation.matrix();
body.translation() += position;
// 3.
target.set_body(body);
return true;
}
Line3D operator*(const Eigen::Isometry3d& t, const Line3D& line){
Matrix6d A=Matrix6d::Zero();
A.block<3,3>(0,0)=t.linear();
A.block<3,3>(0,3)= _skew(t.translation())*t.linear();
A.block<3,3>(3,3)=t.linear();
Vector6d v = (Vector6d)line;
// cout << "v" << endl << v << endl;
// cout << "A" << endl << A << endl;
// cout << "Av" << endl << A*v << endl;
return Line3D(A*v);
}
BaseSensorDataNode* SyncSensorDataNodeMaker::makeNode(MapManager* manager, BaseSensorData* data) {
SynchronizedSensorData* sdata = dynamic_cast<SynchronizedSensorData*>(data);
if (! sdata)
return 0;
SyncSensorDataNode* snode = new SyncSensorDataNode(manager, sdata);
for (size_t i = 0; i<sdata->sensorDatas.size(); i++) {
IMUData* imu = dynamic_cast<IMUData*>(sdata->sensorDatas[i]);
if (imu) {
MapNodeUnaryRelation* imuRel = new MapNodeUnaryRelation(manager);
imuRel->nodes()[0] = snode;
Eigen::Isometry3d t;
t.setIdentity();
t.linear() = imu->orientation().toRotationMatrix();
Eigen::Matrix<double, 6, 6> info;
info.setZero();
info.block<3,3>(3,3) = imu->orientationCovariance().inverse();
//info.block<3,3>(3,3).setIdentity();
imuRel->setTransform(t);
imuRel->setInformationMatrix(info);
snode->setImu(imuRel);
break;
}
}
return snode;
}
Eigen::Isometry3d getRelativeTransform(TagMatch const& match, Eigen::Matrix3d const & camera_matrix, const Eigen::Vector4d &distortion_coefficients, double tag_size)
{
std::vector<cv::Point3f> objPts;
std::vector<cv::Point2f> imgPts;
double s = tag_size/2.;
objPts.push_back(cv::Point3f(-s,-s, 0));
objPts.push_back(cv::Point3f( s,-s, 0));
objPts.push_back(cv::Point3f( s, s, 0));
objPts.push_back(cv::Point3f(-s, s, 0));
imgPts.push_back(match.p0);
imgPts.push_back(match.p1);
imgPts.push_back(match.p2);
imgPts.push_back(match.p3);
cv::Mat rvec, tvec;
cv::Matx33f cameraMatrix(
camera_matrix(0,0), 0, camera_matrix(0,2),
0, camera_matrix(1,1), camera_matrix(1,2),
0, 0, 1);
cv::Vec4f distParam(distortion_coefficients(0), distortion_coefficients(1), distortion_coefficients(2), distortion_coefficients(3));
cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
cv::Matx33d r;
cv::Rodrigues(rvec, r);
Eigen::Matrix3d wRo;
wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);
Eigen::Isometry3d T;
T.linear() = wRo;
T.translation() << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
return T;
}
//==============================================================================
Eigen::Vector6d FreeJoint::convertToPositions(const Eigen::Isometry3d& _tf)
{
Eigen::Vector6d x;
x.head<3>() = math::logMap(_tf.linear());
x.tail<3>() = _tf.translation();
return x;
}
//==============================================================================
btTransform convertTransform(const Eigen::Isometry3d& _T)
{
btTransform trans;
trans.setOrigin(convertVector3(_T.translation()));
trans.setBasis(convertMatrix3x3(_T.linear()));
return trans;
}
Eigen::Isometry3d toIsometry3d(const std::string& _str)
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::Vector6d elements = Eigen::Vector6d::Zero();
std::vector<std::string> pieces;
std::string trimedStr = boost::trim_copy(_str);
boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on);
assert(pieces.size() == 6);
for (size_t i = 0; i < pieces.size(); ++i)
{
if (pieces[i] != "")
{
try
{
elements(i) = boost::lexical_cast<double>(pieces[i].c_str());
}
catch(boost::bad_lexical_cast& e)
{
std::cerr << "value ["
<< pieces[i]
<< "] is not a valid double for SE3["
<< i
<< "]"
<< std::endl;
}
}
}
T.linear() = math::eulerXYZToMatrix(elements.tail<3>());
T.translation() = elements.head<3>();
return T;
}
//==============================================================================
void ShapeNode::setRelativeRotation(const Eigen::Matrix3d& rotation)
{
Eigen::Isometry3d transform = getRelativeTransform();
transform.linear() = rotation;
setRelativeTransform(transform);
}
bool RosMessageContext::getOdomPose(Eigen::Isometry3d& _trans, double time){
bool transformFound = true;
_tfListener->waitForTransform(_odomReferenceFrameId, _baseReferenceFrameId,
ros::Time(time), ros::Duration(1.0));
try{
tf::StampedTransform t;
_tfListener->lookupTransform(_odomReferenceFrameId, _baseReferenceFrameId,
ros::Time(time), t);
Eigen::Isometry3d transform;
transform.translation().x()=t.getOrigin().x();
transform.translation().y()=t.getOrigin().y();
transform.translation().z()=t.getOrigin().z();
Eigen::Quaterniond rot;
rot.x()=t.getRotation().x();
rot.y()=t.getRotation().y();
rot.z()=t.getRotation().z();
rot.w()=t.getRotation().w();
transform.linear()=rot.toRotationMatrix();
_trans = transform;
transformFound = true;
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
transformFound = false;
}
return transformFound;
}
//==============================================================================
fcl::Transform3f FCLTypes::convertTransform(const Eigen::Isometry3d& _T)
{
fcl::Transform3f trans;
trans.setTranslation(convertVector3(_T.translation()));
trans.setRotation(convertMatrix3x3(_T.linear()));
return trans;
}
Eigen::Vector3d RevoluteJoint::getWorldAxis() const
{
Eigen::Isometry3d parentTransf = Eigen::Isometry3d::Identity();
if (this->mParentBodyNode != NULL)
parentTransf = mParentBodyNode->getWorldTransform();
return parentTransf.linear() * mT_ParentBodyToJoint.linear() * mAxis;
}
bool LEG_AIK_T::run(robot::robot_state_t& target, control_data_t* data)
{
assert(data);
if(!data->joystick_ok)
return false;
// 1. Set up
kinematics::Skeleton* robot = data->robot;
robot::robot_kinematics_t* robot_kin = data->kin;
int ms = data->manip_side;
int mi = ms ? robot::MANIP_L_FOOT : robot::MANIP_R_FOOT;
Eigen::Isometry3d Twfoot = data->manip_xform[mi];
Eigen::VectorXd save = target.dart_pose();
assert(robot);
assert(robot_kin);
assert(ms == 1 || ms == 0);
// 2. Target xform
Twfoot.linear() = Twfoot.linear() * data->joy_rotation;
Twfoot.translation() += data->joy_position;
// 3. Run IK
bool ok = robot_kin->leg_ik(Twfoot, ms, target);
if(!ok)
std::cout << "LEG_AIK: outside workspace\n";
assert(target.check_limits());
// 4. Save?
if (!ok)
target.set_dart_pose(save);
else
data->manip_xform[mi] = Twfoot;
data->manip_target = Twfoot;
return ok;
}
/**
* @function param2Transf
* @brief Read a SQ_params instance and get the Tf
*/
Eigen::Isometry3d param2Transf( const SQ_params &_par ) {
Eigen::Isometry3d Tf = Eigen::Isometry3d::Identity();
Tf.translation() << _par.px, _par.py, _par.pz;
Eigen::Matrix3d rot;
rot = Eigen::AngleAxisd(_par.ya, Eigen::Vector3d::UnitZ() )*
Eigen::AngleAxisd( _par.pa, Eigen::Vector3d::UnitY() )*
Eigen::AngleAxisd( _par.ra, Eigen::Vector3d::UnitX() );
Tf.linear() = rot;
return Tf;
}
int main() {
Eigen::Isometry3d pose;
while(1) {
fastrak.getPose(pose, 1);
std::cout << "pose = \n" << Eigen::Quaterniond(pose.linear()).vec().transpose() << std::endl;
}
}
const Eigen::Matrix3d CMiniVisionToolbox::getEssential( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
//ds compute essential matrix: http://en.wikipedia.org/wiki/Essential_matrix TODO check math!
const Eigen::Isometry3d matTransformation( p_matTransformationTo.inverse( )*p_matTransformationFrom );
//const Eigen::Isometry3d matTransformation( p_matTransformationFrom.inverse( )*p_matTransformationTo );
//const Eigen::Isometry3d matTransformation( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
const Eigen::Matrix3d matSkewTranslation( CMiniVisionToolbox::getSkew( matTransformation.translation( ) ) );
//ds compute essential matrix
return matTransformation.linear( )*matSkewTranslation;
}
//==============================================================================
dart::collision::fcl::Transform3 FCLTypes::convertTransform(const Eigen::Isometry3d& _T)
{
#if FCL_VERSION_AT_LEAST(0,6,0)
return _T;
#else
dart::collision::fcl::Transform3 trans;
trans.setTranslation(convertVector3(_T.translation()));
trans.setRotation(convertMatrix3x3(_T.linear()));
return trans;
#endif
}
void G2oOptimizer::addFeatureEdge(const SlamEdge &edge)
{
if (node_id_to_g2o_map_.left.find(edge.id_from_) == node_id_to_g2o_map_.left.end() ||
node_id_to_g2o_map_.left.find(edge.id_to_) == node_id_to_g2o_map_.left.end()) {
ROS_DEBUG("did not find a node of edge %s", edge.id_.c_str());
edge_filter_->remove(edge.id_);
return;
}
if (static_cast<g2o::VertexSE3*>(node_id_to_g2o_map_.left.at(edge.id_from_))->fixed() &&
static_cast<g2o::VertexSE3*>(node_id_to_g2o_map_.left.at(edge.id_to_))->fixed()) {
edge_id_to_g2o_map_.insert(g2o_edge_bimap::value_type(edge.id_, NULL));
return;
}
g2o::EdgeSE3* edge_sensor = new g2o::EdgeSE3();
edge_sensor->setId(optimizer_count_);
edge_sensor->vertices()[0] = node_id_to_g2o_map_.left.at(edge.id_from_);
edge_sensor->vertices()[1] = node_id_to_g2o_map_.left.at(edge.id_to_);
Eigen::Isometry3d transform = edge.displacement_from_ * sensor_transforms_[edge.sensor_from_] * edge.transform_ * sensor_transforms_[edge.sensor_to_].inverse() * edge.displacement_to_.inverse();
if (config_.optimize_xy_only) {
Eigen::Vector3d rpy = g2o::internal::toEuler(transform.linear());
rpy(0) = 0;
rpy(1) = 0;
transform.linear() = g2o::internal::fromEuler(rpy);
transform.translation()(2) = 0;
}
edge_sensor->setMeasurement(transform);
edge_sensor->information() = edge.information_;
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber();
rk->setDelta(1.);
edge_sensor->setRobustKernel(rk);
optimizer_.addEdge(edge_sensor);
edge_id_to_g2o_map_.insert(g2o_edge_bimap::value_type(edge.id_, dynamic_cast<g2o::HyperGraph::Edge*>(edge_sensor)));
optimizer_count_++;
}
GTEST_TEST(TestLcmUtil, testPose) {
default_random_engine generator;
generator.seed(0);
Eigen::Isometry3d pose;
pose.linear() = drake::math::UniformlyRandomRotmat(generator);
pose.translation().setLinSpaced(0, drake::kSpaceDimension);
pose.makeAffine();
const Eigen::Isometry3d& const_pose = pose;
bot_core::position_3d_t msg;
EncodePose(const_pose, msg);
Eigen::Isometry3d pose_back = DecodePose(msg);
EXPECT_TRUE(CompareMatrices(pose.matrix(), pose_back.matrix(), 1e-12,
MatrixCompareType::absolute));
}
std::string toString(const Eigen::Isometry3d& _v)
{
std::ostringstream ostr;
ostr.precision(6);
Eigen::Vector3d xyz = math::matrixToEulerXYZ(_v.linear());
ostr << _v.translation()(0) << " "
<< _v.translation()(1) << " "
<< _v.translation()(2) << " ";
ostr << xyz[0] << " " << xyz[1] << " " << xyz[2];
return ostr.str();
}
geometry_msgs::Pose GraphGridMapper::affineTransformToPoseMsg(const Eigen::Isometry3d &pose)
{
Eigen::Vector3d position = pose.translation();
Eigen::Quaterniond orientation(pose.linear());
geometry_msgs::Pose poseMsg;
poseMsg.position.x = position(0);
poseMsg.position.y = position(1);
poseMsg.position.z = position(2);
poseMsg.orientation.w = orientation.w();
poseMsg.orientation.x = orientation.x();
poseMsg.orientation.y = orientation.y();
poseMsg.orientation.z = orientation.z();
return poseMsg;
}
bool pronto_vis::interpolateScan(const std::vector<float>& iRanges,
const double iTheta0, const double iThetaStep,
const Eigen::Isometry3d& iPose0,
const Eigen::Isometry3d& iPose1,
std::vector<Eigen::Vector3f>& oPoints) {
const int n = iRanges.size();
if (n < 2) return false;
const double tStep = 1.0/(n-1);
Eigen::Quaterniond q0(iPose0.linear());
Eigen::Quaterniond q1(iPose1.linear());
Eigen::Vector3d pos0(iPose0.translation());
Eigen::Vector3d pos1(iPose1.translation());
oPoints.resize(n);
double t = 0;
double theta = iTheta0;
for (int i = 0; i < n; ++i, t += tStep, theta += iThetaStep) {
Eigen::Quaterniond q = q0.slerp(t,q1);
Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
Eigen::Vector3d pt = iRanges[i]*Eigen::Vector3d(cos(theta), sin(theta), 0);
oPoints[i] = (q*pt + pos).cast<float>();
}
return true;
}
Eigen::Isometry3d fromStampedTransform(const tf::StampedTransform& transform) {
geometry_msgs::TransformStamped msg;
tf::transformStampedTFToMsg(transform, msg);
Eigen::Isometry3d t;
Eigen::Quaterniond q(msg.transform.rotation.w,
msg.transform.rotation.x,
msg.transform.rotation.y,
msg.transform.rotation.z);
t.linear()=q.matrix();
t.translation() = Eigen::Vector3d(msg.transform.translation.x,
msg.transform.translation.y,
msg.transform.translation.z);
return t;
}
math::Jacobian
TemplatedJacobianNode<NodeType>::getJacobianSpatialDeriv(
const Eigen::Vector3d& _offset,
const Frame* _inCoordinatesOf) const
{
if(this == _inCoordinatesOf)
return getJacobianSpatialDeriv(_offset);
Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
T.translation() = T.linear() * -_offset;
return math::AdTJac(
T, static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
}
geometry_msgs::Pose Conversions::toMsg(const Eigen::Isometry3d &pose)
{
// Store the pose in a ROS pose message.
Eigen::Vector3d position = pose.translation();
Eigen::Quaterniond orientation(pose.linear());
geometry_msgs::Pose poseMsg;
poseMsg.position.x = position(0);
poseMsg.position.y = position(1);
poseMsg.position.z = position(2);
poseMsg.orientation.w = orientation.w();
poseMsg.orientation.x = orientation.x();
poseMsg.orientation.y = orientation.y();
poseMsg.orientation.z = orientation.z();
return poseMsg;
}
/**
* @function transf2Params
* @brief Fill par arguments for rot and trans from a _Tf
*/
void transf2Params( const Eigen::Isometry3d &_Tf,
SQ_params &_par ) {
_par.px = _Tf.translation().x();
_par.py = _Tf.translation().y();
_par.pz = _Tf.translation().z();
double r31, r11, r21, r32, r33;
double r, p, y;
r31 = _Tf.linear()(2,0);
r11 = _Tf.linear()(0,0);
r21 = _Tf.linear()(1,0);
r32 = _Tf.linear()(2,1);
r33 = _Tf.linear()(2,2);
p = atan2( -r31, sqrt(r11*r11 + r21*r21) );
y = atan2( r21 / cos(p), r11 / cos(p) );
r = atan2( r32 / cos(p), r33 / cos(p) );
_par.ra = r;
_par.pa = p;
_par.ya = y;
}
/**
* @function getRootTransform
* @brief Return <x,y,z, r, p, y> Remember that get_q() gives you the screw so
* do NOT use it directly
*/
Eigen::Matrix<double, 6, 1> InspectorTab::getRootTransform(dart::dynamics::Skeleton* robot) {
dart::dynamics::Joint *joint = robot->getRootBodyNode()->getParentJoint();
Eigen::Matrix<double, 6, 1> pose;
if(joint->getJointType() == dart::dynamics::Joint::FREE) {
Matrix<double, 6, 1> q = joint->get_q();
Eigen::Isometry3d Tf = dart::math::expMap( joint->get_q() );
pose.head<3>() = Tf.translation();
pose.tail<3>() = dart::math::matrixToEulerXYZ( Tf.linear() );
}
else {
pose = getPoseFromTransform(joint->getTransformFromParentBodyNode());
}
return pose;
}
/**
* @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;
}