void LipmVarHeightPlanner::getAB(const double x0[6], const double u[3], double z0, Eigen::Matrix<double,6,6> &A, Eigen::Matrix<double,6,3> &B) const
{
A.setIdentity();
B.setZero();
double x = x0[XX];
double y = x0[YY];
double z = x0[ZZ] - z0;
double px = u[XX];
double py = u[YY];
double F = u[ZZ];
// A
A(0, 3) = _dt;
A(1, 4) = _dt;
A(2, 5) = _dt;
A(3, 0) = F*_dt/(_mass*z);
A(3, 2) = F*_dt*(px-x)/(_mass*z*z);
A(4, 1) = F*_dt/(_mass*z);
A(4, 2) = F*_dt*(py-y)/(_mass*z*z);
// B
B(3, 0) = -F*_dt/(_mass*z);
B(3, 2) = -(px-x)*_dt/(_mass*z);
B(4, 1) = -F*_dt/(_mass*z);
B(4, 2) = -(py-y)*_dt/(_mass*z);
B(5, 2) = _dt/_mass;
}
void SensorDataNodeMaker::process(Serializable* s) {
put(s);
BaseSensorData* data = dynamic_cast<BaseSensorData*>(s);
if (data && data->topic() == _topic) {
BaseSensorDataNode* currentNode = makeNode(_mapManager, data);
Eigen::Isometry3d currentNodeTransform = data->robotReferenceFrame()->transform();
currentNode->setTransform(currentNodeTransform);
_mapManager->addNode(currentNode);
MapNodeBinaryRelation* odom = 0;
if (_previousNode) {
odom = new MapNodeBinaryRelation(_mapManager);
odom->nodes()[0]=_previousNode;
odom->nodes()[1]=currentNode;
odom->setTransform(_previousNodeTransform.inverse()*currentNodeTransform);
Eigen::Matrix<double, 6, 6> info;
info.setIdentity();
info.block<3,3>(3,3) *= 10;
odom->setInformationMatrix(info);
_mapManager->addRelation(odom);
currentNode->setOdometry(odom);
currentNode->setPreviousNode(_previousNode);
}
put(currentNode);
if (odom)
put(odom);
_previousNode = currentNode;
_previousNodeTransform = currentNodeTransform;
}
}
void PointsKalmanFilterPredictor::predict(double time, const std::vector<Eigen::Vector3d> &u, std::vector<Eigen::Matrix<double, 6, 1> >& mu, std::vector<Eigen::Matrix<double, 6, 6> >& sigma)
{
if (time == 0.)
{
mu = mus_;
sigma = sigmas_;
}
else
{
Eigen::Matrix<double, 6, 6> A;
Eigen::Matrix<double, 6, 3> B;
A.setIdentity();
A.block(0, 3, 3, 3) = Eigen::Matrix3d::Identity() * time;
B.block(0, 0, 3, 3).setIdentity();
B.block(3, 0, 3, 3) = Eigen::Matrix3d::Identity() * time;
mu.resize(mus_.size());
sigma.resize(sigmas_.size());
for (int i=0; i<mu.size(); i++)
{
mu[i] = A * mus_[i] + B * u[i];
sigma[i] = A * sigmas_[i] * A.transpose() + R_;
}
}
}
UncertainTransformation::covariance_t UncertainTransformation::UOplus() const
{
Eigen::Matrix<double,6,6> S;
S.setIdentity();
S.topRightCorner<3,3>() = -sm::kinematics::crossMx(_t_a_b_a);
return S.inverse().eval()*_U*S.transpose().inverse().eval();
}
//-- private methods ----
static void MakeStateTransitionMatrix(
const float dT,
Eigen::Matrix<float, 6, 6> &m)
{
m.setIdentity();
m(0,3) = dT;
m(1,4) = dT;
m(2,5) = dT;
}
inline
Eigen::Matrix<T, R1, C1>
mdivide_left_tri(const Eigen::Matrix<T, R1, C1> &A) {
stan::math::check_square("mdivide_left_tri", "A", A);
int n = A.rows();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
b.setIdentity(n, n);
A.template triangularView<TriView>().solveInPlace(b);
return b;
}
Eigen::MatrixXf Material::GetElasticityMatrix(fem::ProblemType type) const
{
float nu = GetPoissonsRatio();
float E = GetYoungsModulus();
switch (type)
{
case fem::PT_FlatStress:
{
Eigen::Matrix<float, 3, 3> m;
m.setIdentity();
m(0, 1) = nu;
m(1, 0) = nu;
m(2, 2) = (1.0f - nu) / 2.0f;
m *= E / (1.0f - nu * nu);
return m;
}
case fem::PT_FlatStraing:
{
Eigen::Matrix<float, 3, 3> m;
m.setIdentity();
m(0, 1) = nu / (1.0f - nu);
m(1, 0) = nu / (1.0f - nu);
m(2, 2) = (1.0f - 2.0f * nu) / 2.0f / (1.0f - nu);
m *= E * (1.0f - nu) / (1.0f + nu) / (1.0f - 2.0f * nu);
return m;
}
case fem::PT_VolumetricStressStrain:
{
Eigen::Matrix<float, 6, 6> m;
m.setIdentity();
float tmp1 = nu / (1.0f - nu);
float tmp2 = (1.0f - 2.0f * nu) / 2.0f / (1.0f - nu);
m(0, 1) = m(1, 0) = m(2, 0) = m(0, 2) = m(2, 1) = m(1, 2) = tmp1;
m(5, 5) = m(4, 4) = m(3, 3) = tmp2;
m *= E * (1.0f - nu) / (1.0f + nu) / (1.0f - 2.0f * nu);
return m;
}
}
}
g2o::EdgeSE3 * Map3DbaseGraph::getG2OEdge(Transformation * transformation)
{
Eigen::Affine3f eigenTransform(transformation->transformationMatrix);
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat transfoSE3(
Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;
edgeSE3->vertices()[0] = graphoptimizer.vertex(transformation->src->id);
edgeSE3->vertices()[1] = graphoptimizer.vertex(transformation->dst->id);
edgeSE3->setMeasurement(transfoSE3.inverse());
edgeSE3->setInverseMeasurement(transfoSE3);
Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
mat.setIdentity(6,6);
edgeSE3->information() = mat;
return edgeSE3;
}
static bool testApply()
{
bool b, ret;
// apply delta:
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 4, 4> diff;
SE3<double> pose;
pose.set( delta );
delta[ 0 ] = Math::deg2Rad( 1.5 );
delta[ 1 ] = Math::deg2Rad( 1.1 );
delta[ 2 ] = Math::deg2Rad( 1.6 );
delta[ 3 ] = 1;
delta[ 4 ] = 1;
delta[ 5 ] = 1;
pose.apply( delta );
expectedT( 0, 3 ) = delta[ 3 ];
expectedT( 1, 3 ) = delta[ 4 ];
expectedT( 2, 3 ) = delta[ 5 ];
Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
double angle = axis.norm(); axis /= angle;
expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
diff = expectedT - pose.transformation();
ret = b = ( diff.array().abs().sum() / 12 < 0.001 );
if( !b ){
std::cout << expectedT << std::endl;
std::cout << pose.transformation() << std::endl;
std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
}
pose.apply( -delta );
expectedT.setIdentity();
b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
CVTTEST_PRINT( "apply", b );
ret &= b;
return ret;
}
void check_is_diagonal()
{
Eigen::Matrix<fl::Real, Size, Size> m;
m.resize(Dim, Dim);
m.setRandom();
EXPECT_FALSE(fl::is_diagonal(m));
m = m.diagonal().asDiagonal();
EXPECT_TRUE(fl::is_diagonal(m));
m.setIdentity();
m *= 3.;
EXPECT_TRUE(fl::is_diagonal(m));
m(0, Dim - 1) = 2;
EXPECT_FALSE(fl::is_diagonal(m));
}
int main(int, char**) {
using SE3Type = Sophus::SE3<double>;
using SO3Type = Sophus::SO3<double>;
using Point = SE3Type::Point;
double const kPi = Sophus::Constants<double>::pi();
std::vector<SE3Type> se3_vec;
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)));
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0)));
se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5)));
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0)));
se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
Point(0, -0.00000001, 0.0000000001)));
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0)));
se3_vec.push_back(SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0)));
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) *
SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0)));
se3_vec.push_back(
SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) *
SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0)));
for (size_t i = 0; i < se3_vec.size(); ++i) {
bool const passed = test(se3_vec[i], se3_vec[(i + 3) % se3_vec.size()]);
if (!passed) {
std::cerr << "failed!" << std::endl << std::endl;
exit(-1);
}
}
Eigen::Matrix<ceres::Jet<double, 28>, 4, 4> mat;
mat.setIdentity();
std::cout << CreateSE3FromMatrix(mat) << std::endl;
return 0;
}
pcl::simulation::TexturedQuad::TexturedQuad (int width, int height) : width_ (width), height_ (height)
{
program_ = gllib::Program::loadProgramFromFile ("single_texture.vert", "single_texture.frag");
program_->use ();
Eigen::Matrix<float, 4, 4> MVP;
MVP.setIdentity();
program_->setUniform ("MVP", MVP);
glUseProgram (0);
glGenTextures (1, &texture_);
glBindTexture (GL_TEXTURE_2D, texture_);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
glBindTexture (GL_TEXTURE_2D, 0);
}
void Map3DbaseGraph::estimate(){
printf("estimate\n");
printf("nr frames: %i nr trans: %i\n",frames.size(),transformations.size());
for(int i = 0; i < frames.size(); i++){
Eigen::Affine3f eigenTransform(poses.at(i));
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat poseSE3(Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::VertexSE3 * vertexSE3 = new g2o::VertexSE3();
vertexSE3->setId(frames.at(i)->id);
vertexSE3->estimate() = poseSE3;
graphoptimizer.addVertex(vertexSE3);
frames.at(i)->g2oVertex = vertexSE3;
if(i == 0){vertexSE3->setFixed(true);}
}
for(int i = 0; i < transformations.size(); i++){
Transformation * transformation = transformations.at(i);
Eigen::Affine3f eigenTransform(transformation->transformationMatrix);
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat transfoSE3(Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;
edgeSE3->vertices()[0] = graphoptimizer.vertex(transformation->src->id);
edgeSE3->vertices()[1] = graphoptimizer.vertex(transformation->dst->id);
edgeSE3->setMeasurement(transfoSE3.inverse());
edgeSE3->setInverseMeasurement(transfoSE3);
Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
mat.setIdentity(6,6);
edgeSE3->information() = mat;
graphoptimizer.addEdge(edgeSE3);
}
graphoptimizer.initializeOptimization();
graphoptimizer.setVerbose(true);
graphoptimizer.optimize(50);
for(int i = 0; i < frames.size(); i++){
g2o::VertexSE3 * vertexSE3_src = (g2o::VertexSE3*)(graphoptimizer.vertex(frames.at(i)->id));
poses.at(i) = (vertexSE3_src->estimate().to_homogenious_matrix()).cast<float>();
//cout<<i<<endl<<poses.at(i)<<endl;
}
printf("estimate done\n");
}
inline Eigen::Affine3f
pcl::TransformationFromCorrespondences::getTransformation ()
{
//Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Eigen::Matrix<float, 3, 3>& u = svd.matrixU(),
& v = svd.matrixV();
Eigen::Matrix<float, 3, 3> s;
s.setIdentity();
if (u.determinant()*v.determinant() < 0.0f)
s(2,2) = -1.0f;
Eigen::Matrix<float, 3, 3> r = u * s * v.transpose();
Eigen::Vector3f t = mean2_ - r*mean1_;
Eigen::Affine3f ret;
ret(0,0)=r(0,0); ret(0,1)=r(0,1); ret(0,2)=r(0,2); ret(0,3)=t(0);
ret(1,0)=r(1,0); ret(1,1)=r(1,1); ret(1,2)=r(1,2); ret(1,3)=t(1);
ret(2,0)=r(2,0); ret(2,1)=r(2,1); ret(2,2)=r(2,2); ret(2,3)=t(2);
ret(3,0)=0.0f; ret(3,1)=0.0f; ret(3,2)=0.0f; ret(3,3)=1.0f;
return (ret);
}
void setGains(const hrl_pr2_force_ctrl::HybridCartesianGains::ConstPtr &msg) // khawkins
{
// Store gains...
if (msg->p_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kp[i] = msg->p_gains[i];
if (msg->d_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kd[i] = msg->d_gains[i];
// Store force gains... khawkins
if (msg->fp_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfp[i] = msg->fp_gains[i];
if (msg->fi_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfi[i] = msg->fi_gains[i];
if (msg->fi_max_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfi_max[i] = msg->fi_max_gains[i];
// Store selector matricies khawkins
if (msg->force_selector.size() == 6)
for (size_t i = 0; i < msg->force_selector.size(); ++i)
if(msg->force_selector[i]) {
force_selector[i] = 1;
motion_selector[i] = 0;
} else {
force_selector[i] = 0;
motion_selector[i] = 1;
}
// Store frame information
if(!msg->header.frame_id.compare(root_name_))
{
use_tip_frame_ = false;
ROS_INFO("New gains in root frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
root_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
St.setIdentity();
}
else if(!msg->header.frame_id.compare(tip_name_))
{
use_tip_frame_ = true;
ROS_INFO("New gains in tip frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
tip_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
}
else
{
use_tip_frame_ = false;
geometry_msgs::PoseStamped in_root;
in_root.pose.orientation.w = 1.0;
in_root.header.frame_id = msg->header.frame_id;
try {
tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
tf_.transformPose(root_name_, in_root, in_root);
}
catch (const tf::TransformException &ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
return;
}
Eigen::Affine3d t;
tf::poseMsgToEigen(in_root.pose, t);
St <<
t(0,0),t(0,1),t(0,2),0,0,0,
t(1,0),t(1,1),t(1,2),0,0,0,
t(2,0),t(2,1),t(2,2),0,0,0,
0,0,0,t(0,0),t(0,1),t(0,2),
0,0,0,t(1,0),t(1,1),t(1,2),
0,0,0,t(2,0),t(2,1),t(2,2);
St.transposeInPlace();
ROS_INFO("New gains in arbitrary frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
msg->header.frame_id.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
}
}
inline void inverse(Eigen::Matrix<double,R,C> &invA) const {
invA.setIdentity(N_);
_ldltP->solveInPlace(invA);
}
bool HybridForceController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
{
rosrt::init();
node_ = n;
ROS_INFO_STREAM("JTTask controller compiled at " << __TIME__ );
// get name of root and tip from the parameter server
// std::string tip_name; // aleeper: Should use the class member instead!
if (!node_.getParam("root_name", root_name_)){
ROS_ERROR("HybridForceController: No root name found on parameter server (namespace: %s)",
node_.getNamespace().c_str());
return false;
}
if (!node_.getParam("tip_name", tip_name_)){
ROS_ERROR("HybridForceController: No tip name found on parameter server (namespace: %s)",
node_.getNamespace().c_str());
return false;
}
// test if we got robot pointer
assert(robot_state);
robot_state_ = robot_state;
// Chain of joints
if (!chain_.init(robot_state_, root_name_, tip_name_))
return false;
if (!chain_.allCalibrated())
{
ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
return false;
}
// Kinematics
KDL::Chain kdl_chain;
chain_.toKDL(kdl_chain);
kin_.reset(new Kin<Joints>(kdl_chain));
// Cartesian gains
double kp_trans, kd_trans, kp_rot, kd_rot;
if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
!node_.getParam("cart_gains/trans/d", kd_trans))
{
ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
return false;
}
if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
!node_.getParam("cart_gains/rot/d", kd_rot))
{
ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
return false;
}
Kp << kp_trans, kp_trans, kp_trans, kp_rot, kp_rot, kp_rot;
Kd << kd_trans, kd_trans, kd_trans, kd_rot, kd_rot, kd_rot;
// aleeper
use_tip_frame_ = false;
if (!node_.getParam("use_tip_frame", use_tip_frame_)){
ROS_WARN("HybridForceController: use_tip_frame was not specified, assuming 'false': %s)",
node_.getNamespace().c_str());
}
St.setIdentity();
// Force desired khawkins
F_des_.setZero();
F_integ_.setZero();
K_effective_.setZero();
double f_trans_max, f_rot_max;
node_.param("force_trans_max", f_trans_max, 9999.0);
node_.param("force_rot_max", f_rot_max, 9999.0);
F_max_ << f_trans_max, f_trans_max, f_trans_max, f_rot_max, f_rot_max, f_rot_max;
// Force gains khawkins
double kfp_trans, kfp_rot, kfi_trans, kfi_rot, kfi_max_trans, kfi_max_rot;
if (!node_.getParam("force_gains/trans/p", kfp_trans) ||
!node_.getParam("force_gains/trans/i", kfi_trans) ||
!node_.getParam("force_gains/trans/i_max", kfi_max_trans))
{
ROS_ERROR("P and I translational force gains not specified (namespace: %s)", node_.getNamespace().c_str());
return false;
}
if (!node_.getParam("force_gains/rot/p", kfp_rot) ||
!node_.getParam("force_gains/rot/i", kfi_rot) ||
!node_.getParam("force_gains/rot/i_max", kfi_max_rot))
{
ROS_ERROR("P and I rotational force gains not specified (namespace: %s)", node_.getNamespace().c_str());
return false;
}
Kfp << kfp_trans, kfp_trans, kfp_trans, kfp_rot, kfp_rot, kfp_rot;
Kfi << kfi_trans, kfi_trans, kfi_trans, kfi_rot, kfi_rot, kfi_rot;
Kfi_max << kfi_max_trans, kfi_max_trans, kfi_max_trans, kfi_max_rot, kfi_max_rot, kfi_max_rot;
motion_selector = CartVec::Ones();
force_selector = CartVec::Zero();
force_filter_.resize(6);
for(int i=0;i<6;i++) {
force_filter_[i].reset(new filters::FilterChain<double>("double"));
force_filter_[i]->configure("force_filter", node_);
ros::Duration(0.2).sleep();
}
qdot_filter_.resize(Joints);
for(int i=0;i<Joints;i++) {
ros::Duration(0.2).sleep();
qdot_filter_[i].reset(new filters::FilterChain<double>("double"));
qdot_filter_[i]->configure("qdot_filter", node_);
}
xdot_filter_.resize(6);
for(int i=0;i<6;i++) {
ros::Duration(0.2).sleep();
xdot_filter_[i].reset(new filters::FilterChain<double>("double"));
xdot_filter_[i]->configure("xdot_filter", node_);
}
node_.param("pose_command_filter", pose_command_filter_, 1.0);
// Velocity saturation
node_.param("vel_saturation_trans", vel_saturation_trans_, 0.0);
node_.param("vel_saturation_rot", vel_saturation_rot_, 0.0);
node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
node_.param("joint_vel_filter", joint_vel_filter_, 1.0);
// Joint gains
for (int i = 0; i < Joints; ++i)
node_.param("joint_feedforward/" + chain_.getJoint(i)->joint_->name, joint_dd_ff_[i], 0.0);
for (int i = 0; i < Joints; ++i)
node_.param("saturation/" + chain_.getJoint(i)->joint_->name, saturation_[i], 0.0);
// Posture gains
node_.param("k_posture", k_posture_, 1.0);
node_.param("resolution/force", res_force_, 0.01);
node_.param("resolution/position", res_position_, 0.001);
node_.param("resolution/torque", res_torque_, 0.01);
node_.param("resolution/orientation", res_orientation_, 0.001);
// force/torque sensor khawkins
zero_wrench_ = true;
node_.param("gripper_params/mass", gripper_mass_, 1.02074);
node_.param("gripper_params/com_pos_x", gripper_com_[0], -0.00126);
node_.param("gripper_params/com_pos_y", gripper_com_[1], 0.001760);
node_.param("gripper_params/com_pos_z", gripper_com_[2], -0.08532);
std::string analog_in_name;
if (!node_.getParam("force_torque_analog_in", analog_in_name))
{
ROS_ERROR("HybridForceController: No \"analog_in_name\" found on parameter namespace: %s",
node_.getNamespace().c_str());
return false;
}
pr2_hardware_interface::HardwareInterface* hw = robot_state_->model_->hw_;
analog_in_ = hw->getAnalogIn(analog_in_name);
if (analog_in_ == NULL)
{
ROS_ERROR("HybridForceController: Cannot find AnalogIn named \"%s\"",
analog_in_name.c_str());
BOOST_FOREACH(const pr2_hardware_interface::AnalogInMap::value_type &v, hw->analog_ins_)
{
ROS_INFO("AnalogIn : %s", v.first.c_str());
}
return false;
}
void initPrecs()
{
// set up some precision matrices
n2prec << 260312.1329351594, 17615.81091248868, -11716.3738046826,
-260221.3577238563, 3028947.570775249, 284048.6838048229,
17615.81091248783, 369156.349498884, -8122.584888439054,
-4130281.103526653, 265383.1196958761, 523737.7444220608,
-11716.3738046842, -8122.58488844048, 673.3469031685361,
93635.22686723019, -137533.0434459766, -22834.5012408561,
-260221.3577238639, -4130281.103526646, 93635.22686720481,
52493931.52684124, -4078689.933502881, -9475682.025736494,
3028947.570775286, 265383.119695912, -137533.0434459558,
-4078689.933502988, 39416288.19312727, 3894322.443643413,
284048.6838048277, 523737.7444220638, -22834.50124085596,
-9475682.025736755, 3894322.443643621, 50690679.29036696;
n2vprec << 624875.2423563644,-8.596260869004408e-11,10576.14746839753,
-65704.86829639168,10646337.23355757,646569.8439109828,
-1.045228848835824e-10,-2.955857780762017e-10,9.820269042393193e-10,
6.912159733474255e-09,-3.751665644813329e-09,-3.511559043545276e-08,
10576.14746839765,7.860307960072532e-10,224224.9112157905,
-233966.3120641535,77714.35666432518,65704.86829639639,
-65704.86829639156,8.021743269637227e-09,-233966.312064158,
7256072.962556601,-1242408.349188809,197719.0360238712,
10646337.23355758,-6.682398634438869e-09,77714.35666432098,
-1242408.349188721,214456943.0273151,11161674.13523376,
646569.8439109783,-3.356490196892992e-08,65704.86829639817,
197719.0360238167,11161674.13523367, 19698666.98402661;
n2aprec << 229528.3846633453, 886.7480854882738, -10039.08940223746, 62445.98594207098, 2715273.460194867, 106542.6230004076,
886.7480854885912, 319242.7032811134, -6397.916315207351, -3608430.146373766, -49269.13482550202, 582748.417531022,
-10039.08940223649, -6397.916315208951, 565.7603057193751, 69152.18264815415, -117569.9760459389, -16259.89068069827,
62445.98594206382, -3608430.146373736, 69152.1826481162, 47244836.25653829, 1303537.745687656, -9808843.224988466,
2715273.46019485, -49269.13482549335, -117569.9760459207, 1303537.745687651, 35830355.245529, 709155.852370202,
106542.623000413, 582748.4175310251, -16259.89068069991, -9808843.224988459, 709155.8523703497, 48304469.04982638;
n2bprec << 148324.039595044, 222.4623044457281, -19531.70697504873, -10192.06466578297, 1631677.485087357, 60190.82294241861,
222.4623044456828, 200041.4398061978, -4054.812572933995, -2258670.079144401, 29578.86052762273, 799843.0721628161,
-19531.70697504886, -4054.812572933865, 2652.99484259674, 46794.05582115334, -215409.6450292048, -24019.87801347017,
-10192.06466578462, -2258670.079144401, 46794.05582115659, 28945336.2353294, -434508.6610355716, -12934377.41525949,
1631677.485087361, 29578.86052762576, -215409.6450292043, -434508.6610355551, 20018126.98420228, 1153711.950184977,
60190.82294241752, 799843.0721628153, -24019.8780134693, -12934377.41525948, 1153711.950184968, 22955884.81085673;
#if 0
// this has zeros for rot-trans interaction
n2prec << 260312.1329351594, 17615.81091248868, -11716.3738046826, 0.0, 0.0, 0.0,
17615.81091248783, 369156.349498884, -8122.584888439054, 0.0, 0.0, 0.0,
-11716.3738046842, -8122.58488844048, 673.3469031685361, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 52493931.52684124, -4078689.933502881, -9475682.025736494,
0.0, 0.0, 0.0, -4078689.933502988, 39416288.19312727, 3894322.443643413,
0.0, 0.0, 0.0, -9475682.025736755, 3894322.443643621, 50690679.29036696;
n2vprec << 624875.2423563644,-8.596260869004408e-11,10576.14746839753,0,0,0,
-1.045228848835824e-10,-2.955857780762017e-10,9.820269042393193e-10,0,0,0,
10576.14746839765,7.860307960072532e-10,224224.9112157905,0,0,0,
0,0,0, 7256072.962556601,-1242408.349188809,197719.0360238712,
0,0,0, -1242408.349188721,214456943.0273151,11161674.13523376,
0,0,0, 197719.0360238167,11161674.13523367, 19698666.98402661;
n2aprec << 229528.3846633453, 886.7480854882738, -10039.08940223746, 0,0,0,
886.7480854885912, 319242.7032811134, -6397.916315207351, 0,0,0,
-10039.08940223649, -6397.916315208951, 565.7603057193751, 0,0,0,
0,0,0, 47244836.25653829, 1303537.745687656, -9808843.224988466,
0,0,0, 1303537.745687651, 35830355.245529, 709155.852370202,
0,0,0, -9808843.224988459, 709155.8523703497, 48304469.04982638;
n2bprec << 148324.039595044, 222.4623044457281, -19531.70697504873, 0,0,0,
222.4623044456828, 200041.4398061978, -4054.812572933995, 0,0,0,
-19531.70697504886, -4054.812572933865, 2652.99484259674, 0,0,0,
0,0,0, 28945336.2353294, -434508.6610355716, -12934377.41525949,
0,0,0, -434508.6610355551, 20018126.98420228, 1153711.950184977,
0,0,0, -12934377.41525948, 1153711.950184968, 22955884.81085673;
#endif
#if 1
n2prec *= 1.0/100000.0;
n2vprec *= 1.0/100000.0;
n2aprec *= 1.0/100000.0;
n2bprec *= 1.0/100000.0;
#endif
diagprec.setIdentity();
diagprec = diagprec*(1000);
diagprec.diagonal().head(3) *= .0001;
}
void Map3DPlanesGraphv3::estimate(){
printf("estimate\n");
printf("nr frames: %i nr trans: %i\n",frames.size(),transformations.size());
for(unsigned int i = 0; i < frames.size(); i++){
Eigen::Affine3f eigenTransform(poses.at(i));
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat poseSE3(Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::VertexSE3 * vertexSE3 = new g2o::VertexSE3();
vertexSE3->setId(frames.at(i)->id);
vertexSE3->estimate() = poseSE3;
graphoptimizer.addVertex(vertexSE3);
frames.at(i)->g2oVertex = vertexSE3;
if(i == 0){vertexSE3->setFixed(true);}
}
for(unsigned int i = 0; i < transformations.size(); i++){
Transformation * transformation = transformations.at(i);
Eigen::Affine3f eigenTransform(transformation->transformationMatrix);
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat transfoSE3(Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;
edgeSE3->vertices()[0] = graphoptimizer.vertex(transformation->src->id);
edgeSE3->vertices()[1] = graphoptimizer.vertex(transformation->dst->id);
edgeSE3->setMeasurement(transfoSE3.inverse());
edgeSE3->setInverseMeasurement(transfoSE3);
Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
mat.setIdentity(6,6);
edgeSE3->information() = mat;
graphoptimizer.addEdge(edgeSE3);
}
for(unsigned int i = 0; i < mergedPlanes.size(); i++){
if(mergedPlanes.at(i).size() > 1){
printf("merged segment: %i\n",mergedPlanes.at(i).size());
Eigen::Affine3f eigenTransform(Matrix4f::Identity());
Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
g2o::SE3Quat poseSE3(Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
g2o::VertexSE3 * vertexSE3 = new g2o::VertexSE3();
vertexSE3->setId(frames.at(i)->id);
vertexSE3->estimate() = poseSE3;
graphoptimizer.addVertex(vertexSE3);
/*
g2o::VertexPlane * vertexPlane2 = new g2o::VertexPlane();
vertexPlane2->setId(200000+i);
graphoptimizer.addVertex(vertexPlane2);
int min = 1000000000;
*/
for(unsigned int j = 0; j < mergedPlanes.at(i).size(); j++){
pair < RGBDFrame *, Plane * > p = mergedPlanes.at(i).at(j);
RGBDFrame * current_frame = p.first;
Plane * current_p = p.second;
/*
if(min > current_frame->id){
min = current_frame->id;
Matrix4f mat = poses.at(min);
vertexPlane2->rx = current_p->normal_x*mat(0,0) + current_p->normal_y*mat(0,1) + current_p->normal_z*mat(0,2);
vertexPlane2->ry = current_p->normal_x*mat(1,0) + current_p->normal_y*mat(1,1) + current_p->normal_z*mat(1,2);
vertexPlane2->rz = current_p->normal_x*mat(2,0) + current_p->normal_y*mat(2,1) + current_p->normal_z*mat(2,2);
//vertexPlane2->d = current_p->d;//*mat(2,0) + current_p->normal_y*mat(2,1) + current_p->normal_z*mat(2,2);
//vertexPlane2->d = current_p->d;//*mat(2,0) + current_p->normal_y*mat(2,1) + current_p->normal_z*mat(2,2);
}
g2o::EdgeSe3Plane2 * edgeSe3Plane2 = new g2o::EdgeSe3Plane2();
edgeSe3Plane2->vertices()[0] = graphoptimizer.vertex(current_frame->id);
edgeSe3Plane2->vertices()[1] = vertexPlane2;
Matrix4d informationMat = Matrix4d::Identity();
informationMat(3,3) = 0.00000000000000;
edgeSe3Plane2->information() = informationMat;
edgeSe3Plane2->setMeasurement(current_p);
graphoptimizer.addEdge(edgeSe3Plane2);
*/
}
printf("Line: %i\n",__LINE__);
}
}
graphoptimizer.initializeOptimization();
graphoptimizer.setVerbose(true);
graphoptimizer.optimize(150);
for(unsigned int i = 0; i < frames.size(); i++){
g2o::VertexSE3 * vertexSE3_src = (g2o::VertexSE3*)(graphoptimizer.vertex(frames.at(i)->id));
poses.at(i) = (vertexSE3_src->estimate().to_homogenious_matrix()).cast<float>();
//cout<<i<<endl<<poses.at(i)<<endl;
}
}
void writeQueue() {
SensorData* data;
std::ofstream ofG2O(&filename[0]);
geometry_msgs::TransformStamped msg;
int num = 0;
// this is the vertex where we are packing the data
g2o::VertexSE3* activeVertex = 0;
// this is the timestamp of the first measurement added to the vertex
double activeVertexTime=0;
// this is the previous vertex
g2o::VertexSE3* previousVertex = 0;
// this is the timestamp of the first measurement added to the previous vertex
double previousVertexTime=0;
// the last position of the robot (not of the vertex, needed to measure the distances)
Eigen::Isometry3d lastRobotPose;
// set of sensors we packed in the current data.
// We do not want to put 10 camera images of the same camera in the same vertex.
std::set<Sensor*> addedSensors;
Eigen::Vector2d distances(0.,0);
while (true)
{
if(! _queue.empty())
{
data = (SensorData*)_queue.front();
double timeNow = _queue.lastElementTime();
conditionalPrint(annoyingLevel) << "size=" << _queue.size() << " lastTime=" << FIXED(timeNow) << endl;
if (timeNow - data->timeStamp()> initialDelay)
{ // we have enough stuff in the queue
_queue.pop_front();
if (! nptr->ok())
continue;
tf::StampedTransform transform;
bool we_got_transf = false;
try
{
ros::Time timeStamp;
// Get transformation
(*tfListener).lookupTransform("/odom", "/base_link", timeStamp.fromSec(data->timeStamp()), transform);
we_got_transf = true;
}
catch (tf::TransformException & ex)
{
ROS_ERROR("%s", ex.what());
}
if (! we_got_transf)
continue;
Eigen::Isometry3d currentRobotPose = fromStampedTransform(transform);
double currentDataTime = data->timeStamp();
distances += isometry2distance(lastRobotPose.inverse()*currentRobotPose);
double passedTime = currentDataTime-previousVertexTime;
lastRobotPose = currentRobotPose;
conditionalPrint(annoyingLevel) << "distances: " << distances[0] << " " << distances[1] << " " << passedTime << endl;
if (distances[0] < minDistances[0] &&
distances[1] < minDistances[1] &&
passedTime < minTime){
conditionalPrint(annoyingLevel) << "reject: (time/distance)" << endl;
// SKIP THE FRAME
delete data;
data = 0;
continue;
}
if (!activeVertex) {
activeVertex = new g2o::VertexSE3();
activeVertex->setId(num);
activeVertex->setEstimate(fromStampedTransform(transform));
activeVertexTime = currentDataTime;
}
Sensor* sensor = data->sensor();
assert (sensor && "!");
// check if we already packed the data for this kind of sensor
if (addedSensors.count(sensor)){
conditionalPrint(annoyingLevel) << "reject: (sensor) "<< endl;
delete data;
} else {
addedSensors.insert(sensor);
Parameter* parameter = sensor->parameter();
assert (parameter && "!@#");
//data->writeOut(filename);
if (! graph->parameters().getParameter(parameter->id())){
graph->parameters().addParameter(parameter);
graph->saveParameter(ofG2O, parameter);
}
activeVertex->addUserData(data);
data->setDataContainer(activeVertex);
}
// detach the data from the thing
data = 0;
if (currentDataTime - activeVertexTime > vertexTimeWindow) {
conditionalPrint(annoyingLevel) << "flush" << endl;
graph->addVertex(activeVertex);
graph->saveVertex(ofG2O, activeVertex);
if (previousVertex) {
EdgeSE3* e = new EdgeSE3();
e->setVertex(0, previousVertex);
e->setVertex(1, activeVertex);
e->setMeasurementFromState();
Eigen::Matrix<double, 6,6> m;
m.setIdentity();
e->setInformation(m);
graph->addEdge(e);
graph->saveEdge(ofG2O, e);
// JACP: do not do the remove, scan the data list and do a release() of the images which are big. The rest can stay in memory
g2o::HyperGraph::Data* d = previousVertex->userData();
while (d) {
RGBDData* rgbd = dynamic_cast<RGBDData*> (d);
if (rgbd)
rgbd->release();
d = d->next();
}
vertecesQueue.push_back(previousVertex);
}
previousVertex = activeVertex;
previousVertexTime = activeVertexTime;
addedSensors.clear();
activeVertex = 0;
distances.setZero();
num++;
conditionalPrint(defaultLevel) << ".";
}
}
}
usleep(20e3); // queue is emp-ty
}
}
LqrController()
{
_Q.setIdentity();
_R.setIdentity();
reset();
}