/* ************************************************************************** */ void Pose2MobileVetLinArm::forwardKinematics(const Pose2Vector& p, boost::optional<const gtsam::Vector&> v, std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx, boost::optional<std::vector<gtsam::Matrix>&> J_px_p, boost::optional<std::vector<gtsam::Matrix>&> J_vx_p, boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const { if (v) throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented"); if (!v && (vx || J_vx_p || J_vx_v)) throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in " "configuration space"); // space for output px.resize(nr_links()); if (vx) vx->resize(nr_links()); if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof())); if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof())); if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof())); // vehicle & arm base pose Pose3 veh_base, tso_base, arm_base; Matrix63 Hveh_base; Matrix64 Htso_base, Harm_base; if (J_px_p || J_vx_p || J_vx_v) { veh_base = computeBasePose3(p.pose(), Hveh_base); tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_, Htso_base); Matrix6 H_tso_comp; arm_base = tso_base.compose(torso_T_arm_, H_tso_comp); Harm_base = H_tso_comp * Htso_base; } else { veh_base = computeBasePose3(p.pose()); tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_); arm_base = tso_base.compose(torso_T_arm_); } // veh base link px[0] = veh_base; if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base; // torso link px[1] = tso_base; if (J_px_p) (*J_px_p)[1].block<6,4>(0,0) = Htso_base; // arm links vector<Pose3> armjpx; vector<Matrix> Jarm_jpx_jp; arm_.updateBasePose(arm_base); arm_.forwardKinematics(p.configuration().tail(arm_.dof()), boost::none, armjpx, boost::none, J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none); for (size_t i = 0; i < arm_.dof(); i++) { px[i+2] = armjpx[i]; if (J_px_p) { // see compose's jacobian (*J_px_p)[i+2].block<6,4>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base; (*J_px_p)[i+2].block(0,4,6,arm_.dof()) = Jarm_jpx_jp[i]; } } }
void Verify::apply() { print_pre_info(); if(!_valid || _c == NULL) { std::cerr << "Verify not valid. Exiting" << std::endl; return; } int steps = _strides; int samples = _samples; int N = _size; if(_exp){ nr_links = stat::make_exponential_points_array( N, steps, START_POINT ); }else{ nr_links = stat::make_linear_points_array( N, steps, START_POINT ); } link_mean = (nr_links.segment(0, steps) + nr_links.segment(1, steps)) / 2; set_theoretical_values(); Eigen::ArrayXXd binned_chain = Eigen::ArrayXXd::Zero(steps,3*samples); std::vector< std::vector<PFloat> > w = std::vector< std::vector<PFloat> >(steps); for(int i = 0; i < steps; i++) { w[i] = std::vector<PFloat>(samples); } Eigen::ArrayXXd Rg_tmp = Eigen::ArrayXXd::Zero(steps,samples); for(int i = 0; i < samples; i++) { _c->build(N); // Vector containing the weight for each individual link Eigen::VectorXd w_tmp = _c->weights(); // bin values for(int j = 0; j<steps; j++) { Eigen::ArrayXXd sub_chain = _c->as_array( nr_links(j), nr_links(j+1)-nr_links(j) ).transpose(); binned_chain(j,3*i+0) = sub_chain.col(0).mean(); binned_chain(j,3*i+1) = sub_chain.col(1).mean(); binned_chain(j,3*i+2) = sub_chain.col(2).mean(); Rg_tmp(j,i) = _c->Rg(0,floor(link_mean(j))); w[j][i] = mult_weights( w_tmp.segment(0, nr_links(j+1)) ); } if(verbose) { write_to_terminal(N,i,steps); } } Eigen::ArrayXXd R_tmp = Eigen::ArrayXXd::Zero(steps,samples); R = Eigen::ArrayXd::Zero(steps); R_var = Eigen::ArrayXd::Zero(steps); Rg = Eigen::ArrayXd::Zero(steps); Rg_var = Eigen::ArrayXd::Zero(steps); for(int i = 0; i < steps; i++) { for(int j = 0; j < samples; j++) { Eigen::Vector3d pos = binned_chain.block(i,3*j,1,3).transpose(); R_tmp(i,j) = pos.norm(); } } for(int i = 0; i<steps; i++) { Eigen::Vector2d mv = stat::get_mean_and_variance(R_tmp.row(i), w[i]); R(i) = mv(0); R_var(i) = mv(1); mv = stat::get_mean_and_variance(Rg_tmp.row(i), w[i]); Rg(i) = mv(0); Rg_var(i) = mv(1); } write_to_file(); print_post_info(); }
void Verify::write_to_terminal(int N, int i, int j) { double p = ( ( double) i*nr_links(nr_links.size()-1) + j) / ((double) _samples*nr_links(nr_links.size()-1)) * 100.f; std::cout << "\r"<< "["<< std::setw(5) << std::setprecision(1)<< p << std::fixed << "%] " <<"Generating " << _samples <<" " <<dictionary.at(_chain_type) <<"s with " << N << " links. " << std::flush; }
/* ************************************************************************** */ void Pose2MobileArm::forwardKinematics( const Pose2Vector& p, boost::optional<const gtsam::Vector&> v, std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx, boost::optional<std::vector<gtsam::Matrix>&> J_px_p, boost::optional<std::vector<gtsam::Matrix>&> J_vx_p, boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const { if (v) throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented"); if (!v && (vx || J_vx_p || J_vx_v)) throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in " "configuration space"); // space for output px.resize(nr_links()); if (vx) vx->resize(nr_links()); if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof())); if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof())); if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof())); // vehicle & arm base pose Pose3 veh_base, arm_base; Matrix63 Hveh_base, Harm_base; if (J_px_p || J_vx_p || J_vx_v) { veh_base = computeBasePose3(p.pose(), Hveh_base); arm_base = computeBaseTransPose3(p.pose(), base_T_arm_, Harm_base); } else { veh_base = computeBasePose3(p.pose()); arm_base = computeBaseTransPose3(p.pose(), base_T_arm_); } // call arm pose and velocity, for arm links // px[0] = base_pose3; px[i] = arm_base * px_arm[i-1] // vx[0] = v(0:1,0); vx[i] = vx[0] + angular x arm_base_pos + arm_base_rot * vx_arm[i-1] // veh base link px[0] = veh_base; if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base; if (vx) { (*vx)[0] = Vector3((*v)[0], (*v)[1], 0.0); // (*J_vx_p)[0] is zero if (J_vx_v) (*J_vx_v)[0].block<2,2>(0,0) = Matrix2::Identity(); } // arm links vector<Pose3> armjpx; vector<Vector3> armjvx; vector<Matrix> Jarm_jpx_jp, Jarm_jvx_jp, Jarm_jvx_jv; arm_.updateBasePose(arm_base); if (v) { const Vector varm = v->tail(arm_.dof()); arm_.forwardKinematics(p.configuration(), boost::optional<const Vector&>(varm), armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none, J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none, J_vx_p ? boost::optional<vector<Matrix>&>(Jarm_jvx_jp) : boost::none, J_vx_v ? boost::optional<vector<Matrix>&>(Jarm_jvx_jv) : boost::none); } else { arm_.forwardKinematics(p.configuration(), boost::none, armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none, J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none); } for (size_t i = 0; i < arm_.dof(); i++) { px[i+1] = armjpx[i]; if (J_px_p) { // see compose's jacobian (*J_px_p)[i+1].block<6,3>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base; (*J_px_p)[i+1].block(0,3,6,arm_.dof()) = Jarm_jpx_jp[i]; } if (vx) { //(*vx)[i+1] = Vector3((*v)[0], (*v)[1], 0.0) + armjvx[i]; } } }