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(); }