void testDHomogTransInv(int ntests, bool check) {
Isometry3d T;
std::default_random_engine generator;
for (int testnr = 0; testnr < ntests; testnr++) {
Vector4d q = uniformlyRandomQuat(generator);
// T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
T = Quaterniond(q(0), q(1), q(2), q(3));
const int nv = 6;
const int nq = 7;
auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
auto qdot_to_v = MatrixXd::Random(nv, nq).eval();
auto dT = dHomogTrans(T, S, qdot_to_v).eval();
auto dTInv = dHomogTransInv(T, dT);
volatile auto vol = dTInv;
if (check) {
auto dTInvInv = dHomogTransInv(T.inverse(), dTInv);
if (!dT.matrix().isApprox(dTInvInv.matrix(), 1e-10)) {
std::cout << "dTInv:\n" << dTInv << "\n\n";
std::cout << "dT:\n" << dT << "\n\n";
std::cout << "dTInvInv:\n" << dTInvInv << "\n\n";
std::cout << "dTInvInv - dT:\n" << dTInvInv - dT << "\n\n";
throw std::runtime_error("wrong");
}
}
}
}
void parseInertial(shared_ptr<RigidBody> body, XMLElement* node, RigidBodyTree * model)
{
Isometry3d T = Isometry3d::Identity();
XMLElement* origin = node->FirstChildElement("origin");
if (origin)
poseAttributesToTransform(origin, T.matrix());
XMLElement* mass = node->FirstChildElement("mass");
if (mass)
parseScalarAttribute(mass, "value", body->mass);
body->com << T(0, 3), T(1, 3), T(2, 3);
Matrix<double, TWIST_SIZE, TWIST_SIZE> I = Matrix<double, TWIST_SIZE, TWIST_SIZE>::Zero();
I.block(3, 3, 3, 3) << body->mass * Matrix3d::Identity();
XMLElement* inertia = node->FirstChildElement("inertia");
if (inertia) {
parseScalarAttribute(inertia, "ixx", I(0, 0));
parseScalarAttribute(inertia, "ixy", I(0, 1));
I(1, 0) = I(0, 1);
parseScalarAttribute(inertia, "ixz", I(0, 2));
I(2, 0) = I(0, 2);
parseScalarAttribute(inertia, "iyy", I(1, 1));
parseScalarAttribute(inertia, "iyz", I(1, 2));
I(2, 1) = I(1, 2);
parseScalarAttribute(inertia, "izz", I(2, 2));
}
auto bodyI = transformSpatialInertia(T, static_cast<Gradient<Isometry3d::MatrixType, Eigen::Dynamic>::type*>(NULL), I);
body->I = bodyI.value();
}
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;
}
Vector7d toVectorQT(const Isometry3d& t){
Quaterniond q(extractRotation(t));
q.normalize();
Vector7d v;
v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); v[6] = q.w();
v.block<3,1>(0,0) = t.translation();
return v;
}
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);
}
}
Transform3 eigen2Xform(const Isometry3d& B) {
Eigen::Matrix4d me = B.matrix();
mat4_t<real> mz;
for (int i=0; i<4; ++i) {
for (int j=0; j<4; ++j) {
mz(i,j) = me(i,j);
}
}
Transform3 rval;
rval.setFromMatrix(mz);
return rval;
}
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);
}
Pointcloud::Ptr joint(camera &cam, Pointcloud::Ptr points, Isometry3d &T, frame &newframe)
{
Pointcloud::Ptr cloud = map2point(cam, newframe.rgb, newframe.depth);
cout << "combining clouds together" << endl;
Pointcloud::Ptr output(new Pointcloud());
//transform cloud points into the same coordinate system
transformPointCloud(*points, *output, T.matrix());
//put the points together
*output += *cloud;
return output;
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs != 6 || nlhs != 7) {
mexErrMsgIdAndTxt("Drake:testGeometryGradientsmex:BadInputs",
"Usage [dT, dTInv, dAdT, dAdT_transpose, x_norm, "
"dx_norm, ddx_norm] = testGeometryGradientsmex(T, S, "
"qdot_to_v, X, dX, x)");
}
int argnum = 0;
Isometry3d T;
memcpy(T.data(), mxGetPr(prhs[argnum++]),
sizeof(double) * drake::kHomogeneousTransformSize);
auto S = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
auto qdot_to_v =
matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
auto X = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
auto dX = matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
auto x = matlabToEigen<4, 1>(prhs[argnum++]);
auto dT = dHomogTrans(T, S, qdot_to_v).eval();
auto dTInv = dHomogTransInv(T, dT).eval();
auto dAdT = dTransformSpatialMotion(T, X, dT, dX).eval();
auto dAdTInv_transpose = dTransformSpatialForce(T, X, dT, dX).eval();
Vector4d x_norm;
Gradient<Vector4d, 4, 1>::type dx_norm;
Gradient<Vector4d, 4, 2>::type ddx_norm;
drake::math::NormalizeVector(x, x_norm, &dx_norm, &ddx_norm);
int outnum = 0;
plhs[outnum++] = eigenToMatlab(dT);
plhs[outnum++] = eigenToMatlab(dTInv);
plhs[outnum++] = eigenToMatlab(dAdT);
plhs[outnum++] = eigenToMatlab(dAdTInv_transpose);
plhs[outnum++] = eigenToMatlab(x_norm);
plhs[outnum++] = eigenToMatlab(dx_norm);
plhs[outnum++] = eigenToMatlab(ddx_norm);
}
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 computeEdgeSE3PriorGradient(Isometry3d& E,
Matrix6d& J,
const Isometry3d& Z,
const Isometry3d& X,
const Isometry3d& P){
// compute the error at the linearization point
const Isometry3d A = Z.inverse()*X;
const Isometry3d B = P;
const Matrix3d Ra = A.rotation();
const Matrix3d Rb = B.rotation();
const Vector3d tb = B.translation();
E = A*B;
const Matrix3d Re=E.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
J.setZero();
// dte/dt
J.block<3,3>(0,0)=Ra;
// dte/dq =0
// dte/dqj
{
Matrix3d S;
skew(S,tb);
J.block<3,3>(0,3)=Ra*S;
}
// dre/dt =0
// dre/dq
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rb);
Map<Matrix3d> Mx(buf); Mx = Ra*Sx;
Map<Matrix3d> My(buf+9); My = Ra*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Sz;
J.block<3,3>(3,3) = dq_dR * M;
}
}
SE3Quat toSE3Quat(const Isometry3d& t)
{
SE3Quat result(t.matrix().topLeftCorner<3,3>(), t.translation());
return result;
}
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;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
//DEBUG
//cout << "constructModelmex: START" << endl;
//END_DEBUG
char buf[100];
mxArray *pm;
if (nrhs!=1) {
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)");
}
if (isa(prhs[0],"DrakeMexPointer")) { // then it's calling the destructor
destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]);
return;
}
const mxArray* pRBM = prhs[0];
RigidBodyManipulator *model=NULL;
// model->robot_name = get_strings(mxGetProperty(pRBM,0,"name"));
const mxArray* pBodies = mxGetProperty(pRBM,0,"body");
if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid");
int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));
const mxArray* pFrames = mxGetProperty(pRBM,0,"frame");
if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid");
int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));
pm = mxGetProperty(pRBM, 0, "num_positions");
if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","model should have a num_positions field");
int num_positions = static_cast<int>(*mxGetPrSafe(pm));
model = new RigidBodyManipulator(num_positions, num_bodies, num_frames);
for (int i=0; i<model->num_bodies; i++) {
//DEBUG
//cout << "constructModelmex: body " << i << endl;
//END_DEBUG
model->bodies[i]->body_index = i;
pm = mxGetProperty(pBodies,i,"linkname");
mxGetString(pm,buf,100);
model->bodies[i]->linkname.assign(buf,strlen(buf));
pm = mxGetProperty(pBodies,i,"robotnum");
model->bodies[i]->robotnum = (int) mxGetScalar(pm)-1;
pm = mxGetProperty(pBodies,i,"mass");
model->bodies[i]->mass = mxGetScalar(pm);
pm = mxGetProperty(pBodies,i,"com");
if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->com.data(),mxGetPrSafe(pm),sizeof(double)*3);
pm = mxGetProperty(pBodies,i,"I");
if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6);
pm = mxGetProperty(pBodies,i,"position_num");
model->bodies[i]->position_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed
pm = mxGetProperty(pBodies,i,"velocity_num");
model->bodies[i]->velocity_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed
pm = mxGetProperty(pBodies,i,"parent");
if (!pm || mxIsEmpty(pm))
model->bodies[i]->parent = nullptr;
else {
int parent_ind = static_cast<int>(mxGetScalar(pm))-1;
if (parent_ind >= static_cast<int>(model->bodies.size()))
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size());
if (parent_ind>=0)
model->bodies[i]->parent = model->bodies[parent_ind];
}
{
mxGetString(mxGetProperty(pBodies, i, "jointname"), buf, 100);
string jointname;
jointname.assign(buf, strlen(buf));
pm = mxGetProperty(pBodies, i, "Ttree");
// todo: check that the size is 4x4
Isometry3d Ttree;
memcpy(Ttree.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4);
int floating = (int) mxGetScalar(mxGetProperty(pBodies, i, "floating"));
Eigen::Vector3d joint_axis;
pm = mxGetProperty(pBodies, i, "joint_axis");
memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);
double pitch = mxGetScalar(mxGetProperty(pBodies, i, "pitch"));
if (model->bodies[i]->hasParent()) {
unique_ptr<DrakeJoint> joint = createJoint(jointname, Ttree, floating, joint_axis, pitch);
// mexPrintf((model->bodies[i]->getJoint().getName() + ": " + std::to_string(model->bodies[i]->getJoint().getNumVelocities()) + "\n").c_str());
FixedAxisOneDoFJoint* fixed_axis_one_dof_joint = dynamic_cast<FixedAxisOneDoFJoint*>(joint.get());
if (fixed_axis_one_dof_joint != nullptr) {
double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min"));
double joint_limit_max = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_max"));
fixed_axis_one_dof_joint->setJointLimits(joint_limit_min,joint_limit_max);
double damping = mxGetScalar(mxGetProperty(pBodies, i, "damping"));
double coulomb_friction = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_friction"));
double coulomb_window = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_window"));
fixed_axis_one_dof_joint->setDynamics(damping, coulomb_friction, coulomb_window);
}
model->bodies[i]->setJoint(move(joint));
}
// pm = mxGetProperty(pBodies,i,"T_body_to_joint");
// memcpy(model->bodies[i]->T_body_to_joint.data(),mxGetPrSafe(pm),sizeof(double)*4*4);
}
//DEBUG
//cout << "constructModelmex: About to parse collision geometry" << endl;
//END_DEBUG
pm = mxGetProperty(pBodies,i,"collision_geometry");
Matrix4d T;
if (!mxIsEmpty(pm)){
for (int j=0; j<mxGetNumberOfElements(pm); j++) {
//DEBUG
//cout << "constructModelmex: Body " << i << ", Element " << j << endl;
//END_DEBUG
mxArray* pShape = mxGetCell(pm,j);
char* group_name_cstr = mxArrayToString(mxGetProperty(pShape,0,"name"));
string group_name;
if (group_name_cstr) {
group_name = group_name_cstr;
} else {
group_name = "default";
}
// Get element-to-link transform from MATLAB object
memcpy(T.data(), mxGetPrSafe(mxGetProperty(pShape,0,"T")), sizeof(double)*4*4);
auto shape = (DrakeShapes::Shape)static_cast<int>(mxGetScalar(mxGetProperty(pShape,0,"drake_shape_id")));
vector<double> params_vec;
RigidBody::CollisionElement element(T, model->bodies[i]);
switch (shape) {
case DrakeShapes::BOX:
{
double* params = mxGetPrSafe(mxGetProperty(pShape,0,"size"));
element.setGeometry(DrakeShapes::Box(Vector3d(params[0],params[1],params[2])));
}
break;
case DrakeShapes::SPHERE:
{
double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius")));
element.setGeometry(DrakeShapes::Sphere(r));
}
break;
case DrakeShapes::CYLINDER:
{
double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius")));
double l(*mxGetPrSafe(mxGetProperty(pShape,0,"len")));
element.setGeometry(DrakeShapes::Cylinder(r, l));
}
break;
case DrakeShapes::MESH:
{
string filename(mxArrayToString(mxGetProperty(pShape,0,"filename")));
element.setGeometry(DrakeShapes::Mesh(filename, filename));
}
break;
case DrakeShapes::MESH_POINTS:
{
mxArray* pPoints;
mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints");
int n_pts = static_cast<int>(mxGetN(pPoints));
Map<Matrix3Xd> pts(mxGetPrSafe(pPoints),3,n_pts);
element.setGeometry(DrakeShapes::MeshPoints(pts));
mxDestroyArray(pPoints);
// The element-to-link transform is applied in
// RigidBodyMesh/getPoints - don't apply it again!
T = Matrix4d::Identity();
}
break;
case DrakeShapes::CAPSULE:
{
double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius")));
double l(*mxGetPrSafe(mxGetProperty(pShape,0,"len")));
element.setGeometry(DrakeShapes::Capsule(r, l));
}
break;
default:
// intentionally do nothing..
//DEBUG
//cout << "constructModelmex: SHOULD NOT GET HERE" << endl;
//END_DEBUG
break;
}
//DEBUG
//cout << "constructModelmex: geometry = " << geometry.get() << endl;
//END_DEBUG
model->addCollisionElement(element, model->bodies[i], group_name);
}
if (!model->bodies[i]->hasParent()) {
model->updateCollisionElements(model->bodies[i]); // update static objects only once - right here on load
}
// Set collision filtering bitmasks
pm = mxGetProperty(pBodies,i,"collision_filter");
DrakeCollision::bitmask group, mask;
mxArray* belongs_to = mxGetField(pm,0,"belongs_to");
mxArray* ignores = mxGetField(pm,0,"ignores");
if (!(mxIsLogical(belongs_to)) || !isMxArrayVector(belongs_to)) {
cout << "is logical: " << mxIsLogical(belongs_to) << endl;
cout << "number of dimensions: " << mxGetNumberOfDimensions(belongs_to) << endl;
mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct",
"The 'belongs_to' field of the 'collision_filter' "
"struct must be a logical vector.");
}
if (!(mxIsLogical(ignores)) || !isMxArrayVector(ignores)) {
mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct",
"The 'ignores' field of the 'collision_filter' "
"struct must be a logical vector.");
}
size_t numel_belongs_to(mxGetNumberOfElements(belongs_to));
size_t numel_ignores(mxGetNumberOfElements(ignores));
size_t num_collision_filter_groups = max(numel_belongs_to, numel_ignores);
if (num_collision_filter_groups > MAX_NUM_COLLISION_FILTER_GROUPS) {
mexErrMsgIdAndTxt("Drake:constructModelmex:TooManyCollisionFilterGroups",
"The total number of collision filter groups (%d) "
"exceeds the maximum allowed number (%d)",
num_collision_filter_groups,
MAX_NUM_COLLISION_FILTER_GROUPS);
}
mxLogical* logical_belongs_to = mxGetLogicals(belongs_to);
for (int j = 0; j < numel_belongs_to; ++j) {
if (static_cast<bool>(logical_belongs_to[j])) {
group.set(j);
}
}
mxLogical* logical_ignores = mxGetLogicals(ignores);
for (int j = 0; j < numel_ignores; ++j) {
if (static_cast<bool>(logical_ignores[j])) {
mask.set(j);
}
}
model->bodies[i]->setCollisionFilter(group,mask);
}
}
// THIS IS UGLY: I'm sending the terrain contact points into the
// contact_pts field of the cpp RigidBody objects
//DEBUG
//cout << "constructModelmex: Parsing contact points " << endl;
//cout << "constructModelmex: Get struct" << endl;
//END_DEBUG
mxArray* contact_pts_struct[1];
if (~mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) {
//DEBUG
//cout << "constructModelmex: Got struct" << endl;
//if (contact_pts_struct) {
//cout << "constructModelmex: Struct pointer: " << contact_pts_struct << endl;
//} else {
//cout << "constructModelmex: Struct pointer NULL" << endl;
//}
//cout << "constructModelmex: Get numel of struct" << endl;
//END_DEBUG
const int n_bodies_w_contact_pts = static_cast<int>(mxGetNumberOfElements(contact_pts_struct[0]));
//DEBUG
//cout << "constructModelmex: Got numel of struct:" << n_bodies_w_contact_pts << endl;
//END_DEBUG
mxArray* pPts;
int body_idx;
int n_pts;
for (int j=0; j < n_bodies_w_contact_pts; j++) {
//DEBUG
//cout << "constructModelmex: Loop: Iteration " << j << endl;
//cout << "constructModelmex: Get body_idx" << endl;
//END_DEBUG
body_idx = (int) mxGetScalar(mxGetField(contact_pts_struct[0],j,"idx")) - 1;
//DEBUG
//cout << "constructModelmex: Got body_idx: " << body_idx << endl;
//cout << "constructModelmex: Get points" << endl;
//END_DEBUG
pPts = mxGetField(contact_pts_struct[0],j,"pts");
//DEBUG
//cout << "constructModelmex: Get points" << endl;
//cout << "constructModelmex: Get number of points" << endl;
//END_DEBUG
n_pts = static_cast<int>(mxGetN(pPts));
//DEBUG
//cout << "constructModelmex: Got number of points: " << n_pts << endl;
//cout << "constructModelmex: Set contact_pts of body" << endl;
//END_DEBUG
Map<Matrix3Xd> pts(mxGetPrSafe(pPts),3,n_pts);
model->bodies[body_idx]->contact_pts = pts;
//DEBUG
//cout << "constructModelmex: Contact_pts of body: " << endl;
//cout << model->bodies[body_idx]->contact_pts << endl;
//END_DEBUG
}
}
for (int i=0; i<model->num_frames; i++) {
pm = mxGetProperty(pFrames,i,"name");
mxGetString(pm,buf,100);
model->frames[i].name.assign(buf,strlen(buf));
pm = mxGetProperty(pFrames,i,"body_ind");
model->frames[i].body_ind = (int) mxGetScalar(pm)-1;
pm = mxGetProperty(pFrames,i,"T");
memcpy(model->frames[i].Ttree.data(),mxGetPrSafe(pm),sizeof(double)*4*4);
}
const mxArray* a_grav_array = mxGetProperty(pRBM,0,"gravity");
if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) {
double* p = mxGetPrSafe(a_grav_array);
model->a_grav[3] = p[0];
model->a_grav[4] = p[1];
model->a_grav[5] = p[2];
} else {
mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object.");
}
// LOOP CONSTRAINTS
const mxArray* pLoops = mxGetProperty(pRBM,0,"loop");
int num_loops = static_cast<int>(mxGetNumberOfElements(pLoops));
model->loops.clear();
for (int i=0; i<num_loops; i++)
{
pm = mxGetProperty(pLoops,i,"body1");
int body_A_ind = static_cast<int>(mxGetScalar(pm)-1);
pm = mxGetProperty(pLoops,i,"body2");
int body_B_ind = static_cast<int>(mxGetScalar(pm)-1);
pm = mxGetProperty(pLoops,i,"pt1");
Vector3d pA;
memcpy(pA.data(), mxGetPrSafe(pm), 3*sizeof(double));
pm = mxGetProperty(pLoops,i,"pt2");
Vector3d pB;
memcpy(pB.data(), mxGetPrSafe(pm), 3*sizeof(double));
model->loops.push_back(RigidBodyLoop(model->bodies[body_A_ind], pA, model->bodies[body_B_ind], pB));
}
//ACTUATORS
const mxArray* pActuators = mxGetProperty(pRBM,0,"actuator");
int num_actuators = static_cast<int>(mxGetNumberOfElements(pActuators));
model->actuators.clear();
for (int i=0; i<num_actuators; i++)
{
pm = mxGetProperty(pActuators,i,"name");
mxGetString(pm,buf,100);
pm = mxGetProperty(pActuators,i,"joint");
int joint = static_cast<int>(mxGetScalar(pm)-1);
pm = mxGetProperty(pActuators,i, "reduction");
model->actuators.push_back(RigidBodyActuator(std::string(buf), model->bodies[joint], static_cast<double>(mxGetScalar(pm))));
}
model->compile();
plhs[0] = createDrakeMexPointer((void*)model,"RigidBodyManipulator");
//DEBUG
//cout << "constructModelmex: END" << endl;
//END_DEBUG
}
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;
}
void computeEdgeSE3Gradient(Isometry3d& E,
Matrix6d& Ji,
Matrix6d& Jj,
const Isometry3d& Z,
const Isometry3d& Xi,
const Isometry3d& Xj,
const Isometry3d& Pi,
const Isometry3d& Pj
){
// compute the error at the linearization point
const Isometry3d A=Z.inverse()*Pi.inverse();
const Isometry3d B=Xi.inverse()*Xj;
const Isometry3d C=Pj;
const Isometry3d AB=A*B;
const Isometry3d BC=B*C;
E=AB*C;
const Matrix3d Re=E.rotation();
const Matrix3d Ra=A.rotation();
const Matrix3d Rb=B.rotation();
const Matrix3d Rc=C.rotation();
const Vector3d tc=C.translation();
//const Vector3d tab=AB.translation();
const Matrix3d Rab=AB.rotation();
const Vector3d tbc=BC.translation();
const Matrix3d Rbc=BC.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
Ji.setZero();
Jj.setZero();
// dte/dti
Ji.block<3,3>(0,0)=-Ra;
// dte/dtj
Jj.block<3,3>(0,0)=Ra*Rb;
// dte/dqi
{
Matrix3d S;
skewT(S,tbc);
Ji.block<3,3>(0,3)=Ra*S;
}
// dte/dqj
{
Matrix3d S;
skew(S,tc);
Jj.block<3,3>(0,3)=Rab*S;
}
// dre/dqi
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sxt,Syt,Szt;
skewT(Sxt,Syt,Szt,Rbc);
Map<Matrix3d> Mx(buf); Mx = Ra*Sxt;
Map<Matrix3d> My(buf+9); My = Ra*Syt;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Szt;
Ji.block<3,3>(3,3) = dq_dR * M;
}
// dre/dqj
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rc);
Map<Matrix3d> Mx(buf); Mx = Rab*Sx;
Map<Matrix3d> My(buf+9); My = Rab*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Rab*Sz;
Jj.block<3,3>(3,3) = dq_dR * M;
}
}
int main(int argc, char* argv[])
{
////////////////////////////////////////////////////////////////////////////
/// SETUP WORLD
////////////////////////////////////////////////////////////////////////////
// load a robot
DartLoader dart_loader;
string robot_file = VRC_DATA_PATH;
robot_file = argv[1];
World *mWorld = dart_loader.parseWorld(VRC_DATA_PATH ROBOT_URDF);
SkeletonDynamics *robot = mWorld->getSkeleton("atlas");
atlas_state_t state;
state.init(robot);
atlas_kinematics_t kin;
kin.init(robot);
robot->setPose(robot->getPose().setZero());
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(VRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
mWorld->addSkeleton(ground);
// Zero atlas's feet at ground
kinematics::BodyNode* foot = robot->getNode(ROBOT_LEFT_FOOT);
kinematics::Shape* shoe = foot->getCollisionShape();
double body_z = -shoe->getTransform().matrix()(2,3) + shoe->getDim()(2)/2;
body_z += -foot->getWorldTransform()(2,3);
VectorXd robot_dofs = robot->getPose();
cout << "body_z = " << body_z << endl;
state.dofs(2) = body_z;
robot->setPose(state.dart_pose());
cout << robot->getNode(ROBOT_BODY)->getWorldTransform() << endl;
// Zero ground
double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
ground->getNode("ground1_root")->getVisualizationShape()->setColor(Vector3d(0.5,0.5,0.5));
VectorXd ground_dofs = ground->getPose();
ground_dofs(1) = foot->getWorldTransform()(1,3);
ground_dofs(2) = -ground_z;
cout << "ground z = " << ground_z << endl;
ground->setPose(ground_dofs);
////////////////////////////////////////////////////////////////////////////
/// COM IK
////////////////////////////////////////////////////////////////////////////
Vector3d world_com;
Isometry3d end_effectors[NUM_MANIPULATORS];
IK_Mode mode[NUM_MANIPULATORS];
BodyNode* left_foot = state.robot()->getNode(ROBOT_LEFT_FOOT);
BodyNode* right_foot = state.robot()->getNode(ROBOT_RIGHT_FOOT);
end_effectors[MANIP_L_FOOT] = state.robot()->getNode(ROBOT_LEFT_FOOT)->getWorldTransform();
end_effectors[MANIP_R_FOOT] = state.robot()->getNode(ROBOT_RIGHT_FOOT)->getWorldTransform();
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
world_com = state.robot()->getWorldCOM();
world_com(2) -= 0.1;
Isometry3d body;
state.get_body(body);
body.rotate(AngleAxisd(M_PI/4, Vector3d::UnitY()));
state.set_body(body);
kin.com_ik(world_com, end_effectors, mode, state);
robot->setPose(state.dart_pose());
MyWindow window;
window.setWorld(mWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Robot Viz");
glutMainLoop();
}
Vector6d toVectorET(const Isometry3d& t) {
Vector6d v;
v.block<3,1>(3,0)=toEuler(extractRotation(t));
v.block<3,1>(0,0) = t.translation();
return v;
}
void test()
{
Isometry3d T = Eigen::Isometry3d::Identity();
T(0,0)=1;
T(0,1)=0;
T(0,2)=0;
T(1,0)=0;
T(1,1)=0.707;
T(1,2)=-0.707;
T(2,0)=0;
T(2,1)=0.707;
T(2,2)=0.707;
T(0,3)=0;
T(1,3)=0;
T(2,3)=0.5;
cout<<T.matrix()<<endl;
for (int i = 2; i <=8; i++)
{
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(i);
v->setEstimate( Eigen::Isometry3d::Identity() );
m_globalOptimizer.addVertex(v);
edge->vertices() [0] = m_globalOptimizer.vertex( i-1 );
edge->vertices() [1] = m_globalOptimizer.vertex( i );
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
m_globalOptimizer.addEdge(edge);
}
#if 0
T = Eigen::Isometry3d::Identity();
//T(2,3)=1;
//cout<<T.matrix()<<endl;
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(8);
v->setEstimate( Eigen::Isometry3d::Identity() );
m_globalOptimizer.addVertex(v);
edge->vertices() [0] = m_globalOptimizer.vertex( 1 );
edge->vertices() [1] = m_globalOptimizer.vertex( 8 );
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
m_globalOptimizer.addEdge(edge);
#endif
m_globalOptimizer.save("pa.g2o");
m_globalOptimizer.initializeOptimization();
//m_globalOptimizer.optimize(50);
m_globalOptimizer.save("pa2.g2o");
cout<<"f**k"<<endl;
VertexSE3* vertex = dynamic_cast<VertexSE3*>(m_globalOptimizer.vertex(2));
Isometry3d pose = vertex -> estimate();
//cout << "matrix:" << endl;
cout << pose.matrix() << endl;
}
Isometry3d fromVectorMQT(const Vector6d& v){
Isometry3d t;
t = fromCompactQuaternion(v.block<3,1>(3,0));
t.translation() = v.block<3,1>(0,0);
return t;
}
Isometry3d fromVectorET(const Vector6d& v) {
Isometry3d t;
t = fromEuler(v.block<3,1>(3,0));
t.translation() = v.block<3,1>(0,0);
return t;
}
// functions to handle the toVector of the whole transformations
Vector6d toVectorMQT(const Isometry3d& t) {
Vector6d v;
v.block<3,1>(3,0) = toCompactQuaternion(extractRotation(t));
v.block<3,1>(0,0) = t.translation();
return v;
}
Isometry3d fromVectorQT(const Vector7d& v) {
Isometry3d t;
t=Quaterniond(v[6], v[3], v[4], v[5]).toRotationMatrix();
t.translation() = v.head<3>();
return t;
}
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;
}
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();
}
}
Isometry3d fromSE3Quat(const SE3Quat& t)
{
Isometry3d result = (Eigen::Isometry3d) t.rotation();
result.translation() = t.translation();
return result;
}
