TEST_F(LoadPlanningModelsPr2, InitOK)
{
ASSERT_TRUE(urdf_ok_);
ASSERT_EQ(urdf_model_->getName(), "pr2_test");
robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
robot_state::RobotState ks(kmodel);
ks.setToRandomValues();
ks.setToDefaultValues();
robot_state::Transforms tf(kmodel->getModelFrame());
Eigen::Affine3d t1;
t1.setIdentity();
t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
tf.setTransform(t1, "some_frame_1");
Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0)*Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
tf.setTransform(t2, "some_frame_2");
Eigen::Affine3d t3;
t3.setIdentity();
t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
tf.setTransform(t3, "some_frame_3");
EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));
Eigen::Affine3d x;
x.setIdentity();
tf.transformPose(ks, "some_frame_2", x, x);
EXPECT_TRUE(t2.translation() == x.translation());
EXPECT_TRUE(t2.rotation() == x.rotation());
tf.transformPose(ks, kmodel->getModelFrame(), x, x);
EXPECT_TRUE(t2.translation() == x.translation());
EXPECT_TRUE(t2.rotation() == x.rotation());
x.setIdentity();
tf.transformPose(ks, "r_wrist_roll_link", x, x);
EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
}
bool VisualOdometry::resetposSrvCallback(
Save::Request& request,
Save::Response& response)
{
const std::string& new_tr = request.filename;
// f2b_
double m00,m01,m02,m03,m10,m11,m12,m13,m20,m21,m22,m23,m30,m31,m32,m33;
int res;
res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n)",
&m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
if (res != 16)
{
res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg)",
&m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
if (res != 16)
{
return false;
}
}
// Eigen::Affine3d pose;
// pose(0,0) = m00;
// pose(0,1) = m01;
// pose(0,2) = m02;
// pose(0,3) = m03;
// pose(1,0) = m10;
// pose(1,1) = m11;
// pose(1,2) = m12;
// pose(1,3) = m13;
// pose(2,0) = m20;
// pose(2,1) = m21;
// pose(2,2) = m22;
// pose(2,3) = m23;
// pose(3,0) = m30;
// pose(3,1) = m31;
// pose(3,2) = m32;
// pose(3,3) = m33;
double yaw,pitch,roll;
tf::Matrix3x3 rot(m00,m01,m02,m10,m11,m12,m20,m21,m22);
tf::Matrix3x3 correction(0,0,1,-1,0,0,0,-1,0); //convert from camera pose to world
rot = rot*correction.inverse();
rot.getEulerYPR(yaw,pitch,roll);
Eigen::Affine3d pose;
pose.setIdentity();
pose = Eigen::Translation<double,3>(m03,m13,m23)*
Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX());
// pose = pose * Eigen::AngleAxis<double>( M_PI,Eigen::Vector3d::UnitZ());
tf::TransformEigenToTF(pose,f2b_);
motion_estimation_.resetModel();
// tf::Vector3 ori(m03,m13,m23);
// f2b_.setBasis(rot);
// f2b_.setOrigin(ori);
return true;
}
void mesh_core::PlaneProjection::getFrame(Eigen::Affine3d& frame) const
{
frame.setIdentity();
frame.translation() = origin_;
frame.linear().col(0) = x_axis_;
frame.linear().col(1) = y_axis_;
frame.linear().col(2) = normal_;
}
void Data4Viewer::drawRGBView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);
glMatrixMode ( GL_MODELVIEW );
Eigen::Affine3d tmp; tmp.setIdentity();
Matrix4d mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate
glLoadMatrixd( mv.data() );
_pKinect->_pRGBCamera->renderCameraInLocal(_pKinect->_pCurrFrame->_gRGB, _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate
//PRINTSTR("drawRGBView");
return;
}
TEST(SpherePointContainment, ComplexOutside)
{
shapes::Sphere shape(1.0);
bodies::Body* sphere = new bodies::Sphere(&shape);
sphere->setScale(0.95);
Eigen::Affine3d pose;
pose.setIdentity();
pose.translation() = Eigen::Vector3d(1.0,1.0,1.0);
sphere->setPose(pose);
bool contains = sphere->containsPoint(0.5,0.0,0.0);
delete sphere;
EXPECT_FALSE(contains);
}
void Data4Viewer::drawGlobalView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix(0.1f, 100.f);
glMatrixMode(GL_MODELVIEW);
Eigen::Affine3d tmp; tmp.setIdentity();
Eigen::Matrix4d mMat;
mMat.row(0) = tmp.matrix().row(0);
mMat.row(1) = -tmp.matrix().row(1);
mMat.row(2) = -tmp.matrix().row(2);
mMat.row(3) = tmp.matrix().row(3);
glLoadMatrixd(mMat.data());
GpuMat rgb(_pKinect->_cvmRGB);
_pKinect->_pRGBCamera->renderCameraInLocal(rgb, _pGL.get(), false, NULL, 0.2f, true); //render in model coordinate
//cout<<("drawRGBView");
return;
}
void RotatingRaySensor::prepareFinalPointcloud(){
// Copies current full pointcloud to pointcloud_full.
poseMutex.lock();
Eigen::Affine3d current_pose2 = current_pose;
Eigen::Affine3d rot;
rot.setIdentity();
rot.rotate(config.transf_sensor_rot_to_sensor);
poseMutex.unlock();
std::list<utils::Vector>::iterator it = fromCloud->begin();
pointcloud_full.clear();
pointcloud_full.reserve(fromCloud->size());
base::Vector3d vec_local;
for(int i=0; it != fromCloud->end(); it++, i++) {
// Transforms the pointcloud back from world to current node (see receiveDate()).
// In addition 'transf_sensor_rot_to_sensor' is applied which describes
// the orientation of the sensor in the unturned sensor frame.
vec_local = rot * current_pose2.inverse() * (*it);
pointcloud_full.push_back(vec_local);
}
//LOG_DEBUG("[RotatingRaySensor::prepareFinalPointcloud]: Pointcloud size: %d", pointcloud_full.size());
fromCloud->clear();
}
robot_state::Transforms::Transforms(const std::string &target_frame) : target_frame_(target_frame)
{
Eigen::Affine3d t;
t.setIdentity();
transforms_[target_frame_] = t;
}
void testSimple()
{
ros::NodeHandle nh;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
ros::Publisher pub_state = nh.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 20);
ros::Publisher pub_scene = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
sleep(1);
std::vector<shapes::ShapeConstPtr> attached_shapes(1, shapes::ShapeConstPtr(new shapes::Box(0.2, 0.1, 0.1)));
Eigen::Affine3d t;
t.setIdentity();
std::vector<Eigen::Affine3d> attached_poses(1, t);
std::vector<std::string> touch;
touch.push_back("r_wrist_roll_link");
touch.push_back("r_forearm_link");
touch.push_back("r_gripper_palm_link");
touch.push_back("r_gripper_l_finger_link");
touch.push_back("r_gripper_r_finger_link");
scene->getCurrentState().getLinkState("r_wrist_roll_link")->attachBody("attached", attached_shapes,
attached_poses, touch);
collision_detection::CollisionRequest req;
req.verbose = true;
unsigned int N = 2;
for (unsigned int i = 0 ; i < N ; ++i)
{
collision_detection::CollisionResult res;
do
{
ROS_INFO("Trying new state...");
res.collision = false;
if (i + 1 == N)
scene->getCurrentState().setToDefaultValues();
else
scene->getCurrentState().setToRandomValues();
scene->checkSelfCollision(req, res);
}
while (res.collision);
ROS_INFO("Displaying valid state...");
moveit_msgs::PlanningScene psmsg;
scene->getPlanningSceneMsg(psmsg);
pub_scene.publish(psmsg);
ros::Duration(0.5).sleep();
/*
moveit_msgs::DisplayTrajectory d;
d.model_id = scene->getRobotModel()->getName();
planning_models::robotStateToRobotStateMsg(scene->getCurrentState(), d.robot_state);
pub_state.publish(d);
for (int j = 0 ; j < 10 ; ++j)
{
// ros::spinOnce();
ros::Duration(0.01).sleep();
}
*/
}
sleep(1);
planning_scene::PlanningScenePtr colliding = planning_scene::PlanningScene::clone(scene);
// construct a planning scene with 100 objects and no collisions
random_numbers::RandomNumberGenerator rng;
req.verbose = false;
for (int i = 0 ; i < 100000 ; ++i)
{
t.translation() = Eigen::Vector3d(rng.uniformReal(-1, 1), rng.uniformReal(-1, 1), rng.uniformReal(0, 2));
scene->getWorldNonConst()->clearObjects();
scene->getWorldNonConst()->addToObject("spere1", shapes::ShapeConstPtr(new shapes::Sphere(0.05)), t);
collision_detection::CollisionResult res;
scene->checkCollision(req, res);
if (!res.collision)
{
int x = colliding->getWorld()->getObjectIds().size();
colliding->getWorldNonConst()->addToObject("speres" + boost::lexical_cast<std::string>(x),
shapes::ShapeConstPtr(new shapes::Sphere(0.05)), t);
std::cout << x << "\n";
if (x == 100)
break;
}
}
moveit_msgs::PlanningScene psmsg;
colliding->getPlanningSceneMsg(psmsg);
pub_scene.publish(psmsg);
ros::WallTime start = ros::WallTime::now();
unsigned int M = 1000;
for (unsigned int i = 0 ; i < M ; ++i)
{
collision_detection::CollisionResult res;
colliding->checkCollision(req, res);
if (res.collision)
ROS_ERROR("PROBLEM");
}
ROS_INFO("%lf full-collision checks per second", (double)M / (ros::WallTime::now() - start).toSec());
/*
req.verbose = false;
ros::WallTime start = ros::WallTime::now();
// unsigned int N = 50000;
N = 50000;
for (unsigned int i = 0 ; i < N ; ++i)
{
collision_detection::CollisionResult res;
scene->getCurrentState().setToRandomValues();
scene->checkSelfCollision(req, res);
}
ROS_INFO("%lf self-collision checks per second", (double)N / (ros::WallTime::now() - start).toSec());
*/
}
bool DenseTracker::match(RgbdImagePyramid& reference, RgbdImagePyramid& current, Eigen::Affine3d& transformation)
{
reference.compute(cfg.getNumLevels());
current.compute(cfg.getNumLevels());
bool success = true;
if(!cfg.UseInitialEstimate)
{
transformation.setIdentity();
}
// our first increment is the given guess
Sophus::SE3 inc(transformation.rotation(), transformation.translation());
Revertable<Sophus::SE3> old(last_xi_);
Revertable<Sophus::SE3> initial(inc);
Revertable<AffineTransform> estimate(AffineTransform::Identity());
bool accept = true;
static stopwatch_collection sw_level(5, "l", 100);
static stopwatch_collection sw_it(5, "it@l", 500);
for(itctx_.Level = cfg.FirstLevel; itctx_.Level >= cfg.LastLevel; --itctx_.Level)
{
// reset error after every pyramid level? yes because errors from different levels are not comparable
itctx_.Iteration = 0;
itctx_.Error = 1;
RgbdImage& ref = reference.level(itctx_.Level);
RgbdImage& cur = current.level(itctx_.Level);
const IntrinsicMatrix& intrinsics = intrinsics_[itctx_.Level];
NormalEquationsLeastSquares ls;
Vector6 x;
Vector6 last_x;
sw_level[itctx_.Level].start();
do
{
sw_it[itctx_.Level].start();
estimate.update() = inc.matrix().cast<NumType>() * estimate().matrix();
if(cfg.UseTemporalSmoothing())
{
old.update() = inc.inverse() * old();
}
if(cfg.UseEstimateSmoothing())
{
initial.update() = inc.inverse() * initial();
}
computeLeastSquaresEquationsInverseCompositional(ref, cur, intrinsics, estimate(), ls);
itctx_.LastError = itctx_.Error;
itctx_.Error = ls.error + 0.5 * cfg.Lambda * old().log().squaredNorm() + 0.5 * cfg.Mu * initial().log().squaredNorm();
// accept the last increment?
accept = itctx_.ErrorDiff() > 0.0;
//ROS_DEBUG_STREAM_COND(!accept, itctx_);
// if we get a worse result, we revert the increment and try our luck on the next pyramid level
if(!accept)
{
old.revert();
initial.revert();
estimate.revert();
inc = Sophus::SE3::exp(Vector6::Zero().cast<double>());
break;
}
// calculate new increment
Matrix6x6 A_diagonal = Matrix6x6::Identity();
if(cfg.UseTemporalSmoothing())
{
ls.A += cfg.Lambda * A_diagonal;
ls.b += cfg.Lambda * old().log().cast<NumType>();
}
if(cfg.UseEstimateSmoothing())
{
ls.A += cfg.Mu * A_diagonal;
ls.b += cfg.Mu * initial().log().cast<NumType>();
}
// first estimate rotation on lowest level
//if(itctx_.IsFirstLevel())
//{
// Eigen::Vector3f rot = ls.A.bottomRightCorner(3, 3).ldlt().solve(ls.b.tail(3));
// x.setZero();
// x.tail<3>() = rot;
//}
//else
//{
// ls.solve(x);
//}
ls.solve(x);
if(itctx_.IsLastLevel())
{
// TODO: should only be used if we also accept the solution
last_a_ = ls.A;
}
//ROS_DEBUG_STREAM_COND(accept, itctx_ << ", Increment: " << x.format(YamlArrayFmt));
inc = Sophus::SE3::exp(x.cast<double>());
itctx_.Iteration++;
itctx_.NumConstraints = ls.num_constraints;
sw_it[itctx_.Level].stopAndPrint();
}
while(accept && itctx_.ErrorDiff() > cfg.Precision && !itctx_.IterationsExceeded());
sw_level[itctx_.Level].stopAndPrint();
}
// if last increment wasn't incorporated because of iterations exceeded, incorporate it
if(itctx_.IterationsExceeded())
{
estimate.update() = inc.matrix().cast<NumType>() * estimate().matrix();
}
// log reason for termination on last level
//ROS_DEBUG_STREAM_COND_NAMED(!itctx_.IterationsExceeded() && itctx_.ErrorDiff() > 0.0 && itctx_.ErrorDiff() <= cfg.Precision, "abort", "error_precision");
//ROS_DEBUG_STREAM_COND_NAMED(!itctx_.IterationsExceeded() && itctx_.ErrorDiff() < 0.0, "abort", "error_increase");
//ROS_DEBUG_STREAM_COND_NAMED(itctx_.IterationsExceeded(), "abort", "iteration_exceeded");
if(success)
{
last_xi_ = Sophus::SE3(estimate().rotation().cast<double>(), estimate().translation().cast<double>());
}
transformation = estimate().inverse().cast<double>();
return success;
}
void smurf::Robot::loadFromSmurf(const std::string& path)
{
configmaps::ConfigMap map;
// parse joints from model
boost::filesystem::path filepath(path);
model = smurf_parser::parseFile(&map, filepath.parent_path().generic_string(), filepath.filename().generic_string(), true);
//first we need to create all Frames
for (configmaps::ConfigVector::iterator it = map["frames"].begin(); it != map["frames"].end(); ++it)
{
configmaps::ConfigMap &fr(it->children);
Frame *frame = new Frame(fr["name"]);
availableFrames.push_back(frame);
//std::cout << "Adding additional frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Link>> link: model->links_)
{
Frame *frame = new Frame(link.first);
for(boost::shared_ptr<urdf::Visual> visual : link.second->visual_array)
{
frame->addVisual(*visual);
}
availableFrames.push_back(frame);
//std::cout << "Adding link frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Joint> > jointIt: model->joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointIt.second;
//std::cout << "Adding joint " << joint->name << std::endl;
Frame *source = getFrameByName(joint->parent_link_name);
Frame *target = getFrameByName(joint->child_link_name);
//TODO this might not be set in some cases, perhaps force a check
configmaps::ConfigMap annotations;
for(configmaps::ConfigItem &cv : map["joints"])
{
if(static_cast<std::string>(cv.children["name"]) == joint->name)
{
annotations = cv.children;
}
}
switch(joint->type)
{
case urdf::Joint::FIXED:
{
const urdf::Pose &tr(joint->parent_to_joint_origin_transform);
StaticTransformation *transform = new StaticTransformation(source, target,
Eigen::Quaterniond(tr.rotation.w, tr.rotation.x, tr.rotation.y, tr.rotation.z),
Eigen::Vector3d(tr.position.x, tr.position.y, tr.position.z));
staticTransforms.push_back(transform);
}
break;
case urdf::Joint::FLOATING:
{
DynamicTransformation *transform = new DynamicTransformation(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"));
dynamicTransforms.push_back(transform);
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
base::JointLimitRange limits;
const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
Eigen::Affine3d parentToOriginAff;
parentToOriginAff.setIdentity();
parentToOriginAff.rotate(rot);
parentToOriginAff.translation() = trans;
Joint *smurfJoint = new Joint (source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, parentToOriginAff, joint);
joints.push_back(smurfJoint);
}
break;
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS:
case urdf::Joint::PRISMATIC:
{
base::JointState minState;
minState.position = joint->limits->lower;
minState.effort = 0;
minState.speed = 0;
base::JointState maxState;
maxState.position = joint->limits->upper;
maxState.effort = joint->limits->effort;
maxState.speed = joint->limits->velocity;
base::JointLimitRange limits;
limits.min = minState;
limits.max = maxState;
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
DynamicTransformation *transform = NULL;
Joint *smurfJoint;
// push the correspondent smurf::joint
const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
Eigen::Affine3d parentToOriginAff;
parentToOriginAff.setIdentity();
parentToOriginAff.rotate(rot);
parentToOriginAff.translation() = trans;
if(joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::CONTINUOUS)
{
transform = new RotationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
smurfJoint = (Joint *)transform;
}
else
{
transform = new TranslationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
smurfJoint = (Joint *)transform;
}
dynamicTransforms.push_back(transform);
joints.push_back(smurfJoint);
}
break;
default:
throw std::runtime_error("Smurf: Error, got unhandles Joint type");
}
}
// parse sensors from map
for (configmaps::ConfigVector::iterator it = map["sensors"].begin(); it != map["sensors"].end(); ++it)
{
configmaps::ConfigMap sensorMap = it->children;
smurf::Sensor *sensor = new Sensor(sensorMap["name"], sensorMap["type"], sensorMap["taskInstanceName"], getFrameByName(sensorMap["link"]));
sensors.push_back(sensor);
}
}
