void EdgeSE3OdomDifferentialCalib::computeError() { const VertexSE3* v1 = dynamic_cast<const VertexSE3*>(_vertices[0]); const VertexSE3* v2 = dynamic_cast<const VertexSE3*>(_vertices[1]); const VertexOdomParams* params = dynamic_cast<const VertexOdomParams*>(_vertices[2]); const Isometry3d& x1 = v1->estimate(); const Isometry3d& x2 = v2->estimate(); Isometry3d odom = internal::fromVectorMQT(measurement()); Eigen::Vector3d rpy = g2o::internal::toEuler(odom.linear()); // g2o::MotionMeasurement mma(odom.translation()(0), odom.translation()(1), rpy(2), diff_time_); // g2o::VelocityMeasurement vel = g2o::OdomConvert::convertToVelocity(mma); // vel.setVl(params->estimate()(0) * vel.vl()); // vel.setVr(params->estimate()(1) * vel.vr()); // g2o::MotionMeasurement mmb = g2o::OdomConvert::convertToMotion(vel, params->estimate()(2)); // odom.translation()(0) = mmb.x(); // odom.translation()(1) = mmb.y(); // rpy(2) = mmb.theta(); // odom.linear() = g2o::internal::fromEuler(rpy); // move to cpp file // implement translation scale // implement rotation scale, which affects odometry translation double drift_theta = params->estimate()(1) * fabs(rpy(2)); double drift_trans = params->estimate()(2) * odom.translation().norm(); Eigen::AngleAxisd drift_rot(drift_theta + drift_trans, Eigen::Vector3d::UnitZ()); odom.translation() = params->estimate()(0) * (drift_rot * odom.translation()); rpy(2) += drift_theta + drift_trans; odom.linear() = g2o::internal::fromEuler(rpy); Isometry3d delta = x2.inverse() * x1 * odom; _error = internal::toVectorMQT(delta); }
Eigen::Isometry3d DecodePose(const bot_core::position_3d_t& msg) { Isometry3d ret; ret.translation() = DecodeVector3d(msg.translation); auto quaternion = DecodeQuaternion(msg.rotation); ret.linear() = drake::math::quat2rotmat(quaternion); ret.makeAffine(); return ret; }
Isometry3d PrismaticJoint::jointTransform(double* const q) const { Isometry3d ret; ret.linear().setIdentity(); ret.translation() = q[0] * translation_axis; ret.makeAffine(); return ret; }
Isometry3d RollPitchYawFloatingJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const { Isometry3d ret; auto pos = q.middleRows<SPACE_DIMENSION>(0); auto rpy = q.middleRows<RPY_SIZE>(SPACE_DIMENSION); ret.linear() = rpy2rotmat(rpy); ret.translation() = pos; ret.makeAffine(); return ret; }
Isometry3d RollPitchYawFloatingJoint::jointTransform(double* const q) const { Isometry3d ret; Map<Vector3d> pos(&q[0]); Map<Vector3d> rpy(&q[3]); ret.linear() = rpy2rotmat(rpy); ret.translation() = pos; ret.makeAffine(); return ret; }
//============================================================================== Isometry3d getRandomTransform() { Isometry3d T = Isometry3d::Identity(); const Vector3d rotation = math::randomVector<3>(-DART_PI, DART_PI); const Vector3d position = Vector3d(math::random(-1.0, 1.0), math::random( 0.5, 1.0), math::random(-1.0, 1.0)); T.translation() = position; T.linear() = math::expMapRot(rotation); return T; }
void CollisionInterface::updateBodyNodes() { int numNodes = mNodeMap.size(); for (std::map<BodyNode*, RigidBody*>::iterator it = mNodeMap.begin(); it != mNodeMap.end(); ++it) { BodyNode *bn = it->first; RigidBody *rb = it->second; if (bn->getSkeleton()->getName() == "pinata") continue; Isometry3d W; W.setIdentity(); W.linear() = rb->mOrientation; W.translation() = rb->mPosition; W.makeAffine(); bn->getSkeleton()->getJoint("freeJoint")->setTransformFromParentBodyNode(W); //bn->updateTransform(); bn->getSkeleton()->computeForwardKinematics(true, false, false); } }
void evaluateXYZExpmapCubicSpline(double t, const PiecewisePolynomial<double> &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel) { Vector6d xyzexp = spline.value(t); auto derivative = spline.derivative(); Vector6d xyzexpdot = derivative.value(t); Vector6d xyzexpddot = derivative.derivative().value(t); xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>(); xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>(); Vector3d expmap = xyzexp.tail<3>(); auto quat_grad = expmap2quat(expmap,2); Vector4d quat = quat_grad.value(); body_pose_des.linear() = quat2rotmat(quat); body_pose_des.translation() = xyzexp.head<3>(); Vector4d quat_dot = quat_grad.gradient().value() * xyzexpdot.tail<3>(); quat_grad.gradient().gradient().value().resize(12,3); Matrix<double,12,3> dE = quat_grad.gradient().gradient().value(); Vector3d expdot = xyzexpdot.tail<3>(); Matrix<double,4,3> Edot = matGradMult(dE,expdot); Vector4d quat_ddot = quat_grad.gradient().value()*xyzexpddot.tail<3>() + Edot*expdot; Matrix<double,3,4> M; Matrix<double,12,4> dM; quatdot2angularvelMatrix(quat,M,&dM); xyzdot_angular_vel.tail<3>() = M*quat_dot; xyzddot_angular_accel.tail<3>() = M*quat_ddot + matGradMult(dM,quat_dot)*quat_dot; }
void evaluateXYZExpmapCubicSpline(double t, const PiecewisePolynomial<double> &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel) { Vector6d xyzexp = spline.value(t); auto derivative = spline.derivative(); Vector6d xyzexpdot = derivative.value(t); Vector6d xyzexpddot = derivative.derivative().value(t); // translational part body_pose_des.translation() = xyzexp.head<3>(); xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>(); xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>(); // rotational part auto expmap = xyzexp.tail<3>(); auto expmap_dot = xyzexpdot.tail<3>(); auto expmap_ddot = xyzexpddot.tail<3>(); // construct autodiff version of expmap // autodiff derivatives represent first and second derivative w.r.t. time // TODO(tkoolen): should use 1 instead of dynamic, but causes issues // with eigen on MSVC 32 bit; should be fixed in 3.3 typedef AutoDiffScalar<Matrix<double, Dynamic, 1>> ADScalar; // TODO(tkoolen): should use 1 instead of dynamic, but causes issues // with eigen on MSVC 32 bit; should be fixed in 3.3 typedef AutoDiffScalar<Matrix<ADScalar, Dynamic, 1>> ADScalarSecondDeriv; Matrix<ADScalarSecondDeriv, 3, 1> expmap_autodiff; for (int i = 0; i < expmap_autodiff.size(); i++) { expmap_autodiff(i).value() = expmap(i); expmap_autodiff(i).derivatives().resize(1); expmap_autodiff(i).derivatives()(0) = expmap_dot(i); expmap_autodiff(i).derivatives()(0).derivatives().resize(1); expmap_autodiff(i).derivatives()(0).derivatives()(0) = expmap_ddot(i); } auto quat_autodiff = expmap2quat(expmap_autodiff); Vector4d quat = autoDiffToValueMatrix(autoDiffToValueMatrix(quat_autodiff)); body_pose_des.linear() = quat2rotmat(quat); // angular velocity and acceleration are computed from quaternion derivative // meaning of derivative vectors remains the same: first and second // derivatives w.r.t. time decltype(quat_autodiff) quat_dot_autodiff; for (int i = 0; i < quat_dot_autodiff.size(); i++) { quat_dot_autodiff(i).value() = quat_autodiff(i).derivatives()(0).value(); quat_dot_autodiff(i).derivatives().resize(1); quat_dot_autodiff(i).derivatives()(0).value() = quat_autodiff(i).derivatives()(0).derivatives()(0); quat_dot_autodiff(i).derivatives()(0).derivatives().resize(1); quat_dot_autodiff(i).derivatives()(0).derivatives()(0) = std::numeric_limits<double>::quiet_NaN(); // we're not interested in // second deriv of angular // velocity } auto omega_autodiff = (quatdot2angularvelMatrix(quat_autodiff) * quat_dot_autodiff).eval(); auto omega = xyzdot_angular_vel.tail<3>(); auto omega_dot = xyzddot_angular_accel.tail<3>(); for (int i = 0; i < omega_autodiff.size(); i++) { omega(i) = omega_autodiff(i).value().value(); omega_dot(i) = omega_autodiff(i).derivatives()(0).value(); } }
Vector6d bodySpatialMotionPD( const RigidBodyTree &r, const DrakeRobotState &robot_state, const int body_index, const Isometry3d &body_pose_des, const Ref<const Vector6d> &body_v_des, const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp, const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world) { // @param body_pose_des desired pose in the task frame, this is the // homogeneous transformation from desired body frame to task frame // @param body_v_des desired [xyzdot;angular_velocity] in task frame // @param body_vdot_des desired [xyzddot;angular_acceleration] in task // frame // @param Kp The gain in task frame // @param Kd The gain in task frame // @param T_task_to_world The homogeneous transform from task to world // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame Isometry3d T_world_to_task = T_task_to_world.inverse(); KinematicsCache<double> cache = r.doKinematics(robot_state.q, robot_state.qd); auto body_pose = r.relativeTransform(cache, 0, body_index); const auto &body_xyz = body_pose.translation(); Vector3d body_xyz_task = T_world_to_task * body_xyz; Vector4d body_quat = rotmat2quat(body_pose.linear()); std::vector<int> v_indices; auto J_geometric = r.geometricJacobian(cache, 0, body_index, body_index, true, &v_indices); VectorXd v_compact(v_indices.size()); for (size_t i = 0; i < v_indices.size(); i++) { v_compact(i) = robot_state.qd(v_indices[i]); } Vector6d body_twist = J_geometric * v_compact; Matrix3d R_body_to_world = quat2rotmat(body_quat); Matrix3d R_world_to_body = R_body_to_world.transpose(); Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world; Vector3d body_angular_vel = R_body_to_world * body_twist.head<3>(); // body_angular velocity in world frame Vector3d body_xyzdot = R_body_to_world * body_twist.tail<3>(); // body_xyzdot in world frame Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel; Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot; Vector3d body_xyz_des = body_pose_des.translation(); Vector3d body_angular_vel_des = body_v_des.tail<3>(); Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>(); Vector3d xyz_err_task = body_xyz_des - body_xyz_task; Matrix3d R_des = body_pose_des.linear(); Matrix3d R_err_task = R_des * R_body_to_task.transpose(); Vector4d angleAxis_err_task = rotmat2axis(R_err_task); Vector3d angular_err_task = angleAxis_err_task.head<3>() * angleAxis_err_task(3); Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task; Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task; Vector3d Kp_xyz = Kp.head<3>(); Vector3d Kd_xyz = Kd.head<3>(); Vector3d Kp_angular = Kp.tail<3>(); Vector3d Kd_angular = Kd.tail<3>(); Vector3d body_xyzddot_task = (Kp_xyz.array() * xyz_err_task.array()).matrix() + (Kd_xyz.array() * xyzdot_err_task.array()).matrix() + body_vdot_des.head<3>(); Vector3d body_angular_vel_dot_task = (Kp_angular.array() * angular_err_task.array()).matrix() + (Kd_angular.array() * angular_vel_err_task.array()).matrix() + body_angular_vel_dot_des; Vector6d twist_dot = Vector6d::Zero(); Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task; Vector3d body_angular_vel_dot = T_task_to_world.linear() * body_angular_vel_dot_task; twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot; twist_dot.tail<3>() = R_world_to_body * body_xyzddot - body_twist.head<3>().cross(body_twist.tail<3>()); return twist_dot; }
Vector6d bodySpatialMotionPD(RigidBodyManipulator *r, DrakeRobotState &robot_state, const int body_index, const Isometry3d &body_pose_des, const Ref<const Vector6d> &body_v_des, const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp, const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world) { // @param body_pose_des desired pose in the task frame, this is the homogeneous transformation from desired body frame to task frame // @param body_v_des desired [xyzdot;angular_velocity] in task frame // @param body_vdot_des desired [xyzddot;angular_acceleration] in task frame // @param Kp The gain in task frame // @param Kd The gain in task frame // @param T_task_to_world The homogeneous transform from task to world // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame if(!r->getUseNewKinsol()) { throw std::runtime_error("bodySpatialMotionPD requires new kinsol format"); } Isometry3d T_world_to_task = T_task_to_world.inverse(); r->doKinematicsNew(robot_state.q,robot_state.qd,false); Vector3d origin = Vector3d::Zero(); auto body_pose = r->forwardKinNew(origin,body_index,0,2,0); Vector3d body_xyz = body_pose.value().head<3>(); Vector3d body_xyz_task = T_world_to_task * body_xyz.colwise().homogeneous(); Vector4d body_quat = body_pose.value().tail<4>(); std::vector<int> v_indices; auto J_geometric = r->geometricJacobian<double>(0,body_index,body_index,0,true,&v_indices); VectorXd v_compact(v_indices.size()); for(size_t i = 0;i<v_indices.size();i++) { v_compact(i) = robot_state.qd(v_indices[i]); } Vector6d body_twist = J_geometric.value() * v_compact; Matrix3d R_body_to_world = quat2rotmat(body_quat); Matrix3d R_world_to_body = R_body_to_world.transpose(); Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world; Vector3d body_angular_vel = R_body_to_world * body_twist.head<3>();// body_angular velocity in world frame Vector3d body_xyzdot = R_body_to_world * body_twist.tail<3>();// body_xyzdot in world frame Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel; Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot; Vector3d body_xyz_des = body_pose_des.translation(); Vector3d body_angular_vel_des = body_v_des.tail<3>(); Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>(); Vector3d xyz_err_task = body_xyz_des-body_xyz_task; Matrix3d R_des = body_pose_des.linear(); Matrix3d R_err_task = R_des * R_body_to_task.transpose(); Vector4d angleAxis_err_task = rotmat2axis(R_err_task); Vector3d angular_err_task = angleAxis_err_task.head<3>() * angleAxis_err_task(3); Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task; Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task; Vector3d Kp_xyz = Kp.head<3>(); Vector3d Kd_xyz = Kd.head<3>(); Vector3d Kp_angular = Kp.tail<3>(); Vector3d Kd_angular = Kd.tail<3>(); Vector3d body_xyzddot_task = (Kp_xyz.array() * xyz_err_task.array()).matrix() + (Kd_xyz.array() * xyzdot_err_task.array()).matrix() + body_vdot_des.head<3>(); Vector3d body_angular_vel_dot_task = (Kp_angular.array() * angular_err_task.array()).matrix() + (Kd_angular.array() * angular_vel_err_task.array()).matrix() + body_angular_vel_dot_des; Vector6d twist_dot = Vector6d::Zero(); Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task; Vector3d body_angular_vel_dot = T_task_to_world.linear() * body_angular_vel_dot_task; twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot; twist_dot.tail<3>() = R_world_to_body * body_xyzddot - body_twist.head<3>().cross(body_twist.tail<3>()); return twist_dot; }
bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices) { double err=0; //If my correspondaces indices are less then a minimum threshold, stop it please if ((int)indices.size()<minimalSetSize()) return false; //My initial guess for the transformation it's the identity matrix //maybe in the future i could have a less rough guess transform = Isometry3d::Identity(); //Unroll the matrix to a vector Vector12d x=homogeneous2vector(transform.matrix()); Matrix9x1d rx=x.block<9,1>(0,0); //Initialization of variables, melting fat, i've so much space Matrix9d H; H.setZero(); Vector9d b; b.setZero(); Matrix3x9d A; //iteration for each correspondace for (size_t i=0; i<indices.size(); i++) { A.setZero(); const Correspondence& c = correspondences[indices[i]]; const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge()); //estraggo i vertici dagli edge const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0)); const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1)); //recupero i dati dei piani const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate(); const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate(); //recupeo le normali, mi servono per condizionare la parte rotazionale Vector3d ni; Vector3d nj; Vector4d coeffs_i=pi.toVector(); Vector4d coeffs_j=pj.toVector(); ni=coeffs_i.head(3); nj=coeffs_j.head(3); //inizializzo lo jacobiano, solo la parte rotazionale (per ora) A.block<1,3>(0,0)=nj.transpose(); A.block<1,3>(1,3)=nj.transpose(); A.block<1,3>(2,6)=nj.transpose(); if(DEBUG) { cerr << "[DEBUG] PI"<<endl; cerr << coeffs_i<<endl; cerr << "[DEBUG] PJ "<<endl; cerr << coeffs_j<<endl; cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl; cerr << A<<endl; } //errore //inizializzo errore Vector3d ek; ek.setZero(); ek=A*x.block<9,1>(0,0)-coeffs_i.head(3); H+=A.transpose()*A; b+=A.transpose()*ek; err+=abs(ek.dot(ek)); if(DEBUG) cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl; if(DEBUG) cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl; } LDLT<Matrix9d> ldlt(H); if (ldlt.isNegative()) return false; rx=ldlt.solve(-b); // using a LDLT factorizationldlt; x.block<3,1>(0,0)+=rx.block<3,1>(0,0); x.block<3,1>(3,0)+=rx.block<3,1>(3,0); x.block<3,1>(6,0)+=rx.block<3,1>(6,0); if(DEBUG) { cerr << "Solving the linear system"<<endl; cerr << "------------>H"<<endl; cerr << H<<endl; cerr << "------------>b"<<endl; cerr << b<<endl; cerr << "------------>rx"<<endl; cerr << rx<<endl; cerr << "------------>xTEMP"<<endl; cerr << vector2homogeneous(x)<<endl<<endl; cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl; cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl; cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl; } Matrix4d Xtemp=vector2homogeneous(x); //now the problem si solved but i could have (and probably i have!) //a not orthogonal rotation matrix, so i've to recondition it Matrix3x3d R=Xtemp.block<3,3>(0,0); // recondition the rotation JacobiSVD<Matrix3x3d> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV); if (svd.singularValues()(0)<.5) return false; R=svd.matrixU()*svd.matrixV().transpose(); Isometry3d X = Isometry3d::Identity(); X.linear()=R; X.translation()= x.block<3,1>(0,3); if(DEBUG) cerr << "X dopo il ricondizionamento appare come:"<<endl; if(DEBUG) cerr << X.matrix()<<endl; Matrix3d H2=Matrix3d::Zero(); Vector3d b2=Vector3d::Zero(); Vector3d A2=Vector3d::Zero(); //at this point rotation is cured, now it's time to work on the translation for (size_t i=0; i<indices.size(); i++) { if(DEBUG) cerr << "[TRANSLATION JACOBIAN START][PLANE "<<i<<"]"<<endl; const Correspondence& c = correspondences[indices[i]]; const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge()); //estraggo i vertici dagli edge const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0)); const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1)); //recupero i dati dei piani const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate(); const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate(); //recupeo le normali, mi servono per condizionare la parte rotazionale Vector3d ni; Vector3d nj; Vector4d coeffs_i=pi.toVector(); Vector4d coeffs_j=pj.toVector(); double di; double dj; ni=coeffs_i.head(3); nj=coeffs_j.head(3); di=coeffs_i(3); dj=coeffs_j(3); if(DEBUG) cerr << "[TRANSLATION JACOBIAN][ JACOBIAN IS: ]"<<endl; A2=(-nj.transpose()*R.transpose()); if(DEBUG) cerr << A2<<endl; double ek; ek=dj+A2.transpose()*X.translation()-di; H2+=A2*A2.transpose(); b2+= (A2.transpose()*ek); err += abs(ek*ek); if(DEBUG) cerr << "[TRANSLATIONAL Hessian for plane "<<i<<"]"<<endl<<H2<<endl; if(DEBUG) cerr << "[TRANSLATIONAL B for plane "<<i<<"]"<<endl<<b2<<endl; } if(DEBUG) cerr << "[FINAL][TRANSLATIONAL Hessian]"<<endl<<H2<<endl; if(DEBUG) cerr << "[FINAL][TRANSLATIONAL B]"<<endl<<b2<<endl; //solving the system Vector3d dt; LDLT<Matrix3d> ldlt2(H2); dt=ldlt2.solve(-b2); // using a LDLT factorizationldlt; if(DEBUG) cerr << "[FINAL][TRANSLATIONAL DT]"<<endl<<dt<<endl; X.translation()+=dt; transform = X; if(DEBUG) { cerr << "TRANSFORM found: " << endl<< endl<< endl; cerr << g2o::internal::toVectorMQT(X) << endl;; cerr << transform.matrix()<< endl;; } return true; }