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