Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const { Eigen::Matrix <double,6,3> X_subspace; X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation (); X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation (); return X_subspace; }
Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const { Eigen::Matrix <double,6,3> X_subspace; X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> (); X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> (); X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> (); X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero (); return X_subspace; }
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const { Eigen::Matrix<double,6,1> res; res.head<3>() = m.rotation().col(axis); res.tail<3>() = Motion::Vector3::Zero(); return res; }
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const { /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */ Eigen::Matrix<double,6,1> res; res.tail<3>() = m.rotation() * axis; res.head<3>() = m.translation().cross(res.tail<3>()); return res; }
static inline void writeSE3(YAMLWriter& writer, const SE3& value) { writer.startFlowStyleListing(); const Vector3& p = value.translation(); writer.putScalar(p.x()); writer.putScalar(p.y()); writer.putScalar(p.z()); const Quat& q = value.rotation(); writer.putScalar(q.w()); writer.putScalar(q.x()); writer.putScalar(q.y()); writer.putScalar(q.z()); writer.endListing(); }
bool BodyItemImpl::store(Archive& archive) { archive.setDoubleFormat("% .6f"); archive.writeRelocatablePath("modelFile", self->filePath()); archive.write("currentBaseLink", (currentBaseLink ? currentBaseLink->name() : ""), DOUBLE_QUOTED); /// \todo Improve the following for current / initial position representations write(archive, "rootPosition", body->rootLink()->p()); write(archive, "rootAttitude", Matrix3(body->rootLink()->R())); Listing* qs = archive.createFlowStyleListing("jointPositions"); int n = body->numJoints(); for(int i=0; i < n; ++i){ qs->append(body->joint(i)->q(), 10, n); } //! \todo replace the following code with the ValueTree serialization function of BodyState SE3 initialRootPosition; if(initialState.getRootLinkPosition(initialRootPosition)){ write(archive, "initialRootPosition", initialRootPosition.translation()); write(archive, "initialRootAttitude", Matrix3(initialRootPosition.rotation())); } BodyState::Data& initialJointPositions = initialState.data(BodyState::JOINT_POSITIONS); if(!initialJointPositions.empty()){ qs = archive.createFlowStyleListing("initialJointPositions"); for(int i=0; i < initialJointPositions.size(); ++i){ qs->append(initialJointPositions[i], 10, n); } } write(archive, "zmp", zmp); if(isOriginalModelStatic != body->isStaticModel()){ archive.write("staticModel", body->isStaticModel()); } archive.write("selfCollisionDetection", isSelfCollisionDetectionEnabled); archive.write("isEditable", isEditable); return true; }
ForceTpl se3ActionInverse_impl(const SE3 & m) const { return ForceTpl(m.rotation().transpose()*linear_impl(), m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) ); }
/// af = aXb.act(bf) ForceTpl se3Action_impl(const SE3 & m) const { Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) ); return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl()); }