DRAKERBSYSTEM_EXPORT RigidBodySystem::StateVector<double> Drake::getInitialState(const RigidBodySystem& sys) {
VectorXd x0(sys.tree->num_positions + sys.tree->num_velocities);
default_random_engine generator;
x0 << sys.tree->getRandomConfiguration(generator), VectorXd::Random(sys.tree->num_velocities);
// todo: implement joint limits, etc.
if (sys.tree->getNumPositionConstraints()) {
// todo: move this up to the system level?
OptimizationProblem prog;
std::vector<RigidBodyLoop,Eigen::aligned_allocator<RigidBodyLoop> > const& loops = sys.tree->loops;
int nq = sys.tree->num_positions;
auto qvar = prog.addContinuousVariables(nq);
Matrix<double,7,1> bTbp = Matrix<double,7,1>::Zero(); bTbp(3)=1.0;
Vector2d tspan; tspan<<-std::numeric_limits<double>::infinity(),std::numeric_limits<double>::infinity();
Vector3d zero = Vector3d::Zero();
for (int i=0; i<loops.size(); i++) {
auto con1 = make_shared<RelativePositionConstraint>(sys.tree.get(), zero, zero, zero, loops[i].frameA->frame_index, loops[i].frameB->frame_index,bTbp,tspan);
std::shared_ptr<SingleTimeKinematicConstraintWrapper> con1wrapper(new SingleTimeKinematicConstraintWrapper({qvar},con1));
prog.addConstraint(con1wrapper);
auto con2 = make_shared<RelativePositionConstraint>(sys.tree.get(), loops[i].axis, loops[i].axis, loops[i].axis, loops[i].frameA->frame_index, loops[i].frameB->frame_index,bTbp,tspan);
std::shared_ptr<SingleTimeKinematicConstraintWrapper> con2wrapper(new SingleTimeKinematicConstraintWrapper({qvar},con2));
prog.addConstraint(con2wrapper);
}
VectorXd q_guess = x0.topRows(nq);
prog.addQuadraticCost(MatrixXd::Identity(nq,nq),q_guess);
prog.solve();
x0 << qvar.value(), VectorXd::Zero(sys.tree->num_velocities);
}
return x0;
}
RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(const double& t, const RigidBodySystem::StateVector<double>& x, const RigidBodySystem::InputVector<double>& u) const {
using namespace std;
using namespace Eigen;
eigen_aligned_unordered_map<const RigidBody *, Matrix<double, 6, 1> > f_ext;
// todo: make kinematics cache once and re-use it (but have to make one per type)
auto nq = tree->num_positions;
auto nv = tree->num_velocities;
auto num_actuators = tree->actuators.size();
auto q = x.topRows(nq);
auto v = x.bottomRows(nv);
auto kinsol = tree->doKinematics(q,v);
// todo: preallocate the optimization problem and constraints, and simply update them then solve on each function eval.
// happily, this clunkier version seems fast enough for now
// the optimization framework should support this (though it has not been tested thoroughly yet)
OptimizationProblem prog;
auto const & vdot = prog.addContinuousVariables(nv,"vdot");
auto H = tree->massMatrix(kinsol);
Eigen::MatrixXd H_and_neg_JT = H;
{ // loop through rigid body force elements and accumulate f_ext
// todo: distinguish between AppliedForce and ConstraintForce
// todo: have AppliedForce output tau (instead of f_ext). it's more direct, and just as easy to compute.
int u_index = 0;
const NullVector<double> force_state; // todo: will have to handle this case
for (auto const & prop : props) {
RigidBodyFrame* frame = prop->getFrame();
RigidBody* body = frame->body.get();
int num_inputs = 1; // todo: generalize this
RigidBodyPropellor::InputVector<double> u_i(u.middleRows(u_index,num_inputs));
// todo: push the frame to body transform into the dynamicsBias method?
Matrix<double,6,1> f_ext_i = transformSpatialForce(frame->transform_to_body,prop->output(t,force_state,u_i,kinsol));
if (f_ext.find(body)==f_ext.end()) f_ext[body] = f_ext_i;
else f_ext[body] = f_ext[body]+f_ext_i;
u_index += num_inputs;
}
}
VectorXd C = tree->dynamicsBiasTerm(kinsol,f_ext);
if (num_actuators > 0) C -= tree->B*u.topRows(num_actuators);
{ // apply contact forces
const bool use_multi_contact = false;
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
if (use_multi_contact)
tree->potentialCollisions(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
else
tree->collisionDetect(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
for (int i=0; i<phi.rows(); i++) {
if (phi(i)<0.0) { // then i have contact
double mu = 1.0; // todo: make this a parameter
// todo: move this entire block to a shared an updated "contactJacobian" method in RigidBodyTree
auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i], 0, false);
auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i], 0, false);
Vector3d this_normal = normal.col(i);
// compute the surface tangent basis
Vector3d tangent1;
if (1.0 - this_normal(2) < EPSILON) { // handle the unit-normal case (since it's unit length, just check z)
tangent1 << 1.0, 0.0, 0.0;
} else if (1 + this_normal(2) < EPSILON) {
tangent1 << -1.0, 0.0, 0.0; //same for the reflected case
} else {// now the general case
tangent1 << this_normal(1), -this_normal(0), 0.0;
tangent1 /= sqrt(this_normal(1)*this_normal(1) + this_normal(0)*this_normal(0));
}
Vector3d tangent2 = tangent1.cross(this_normal);
Matrix3d R; // rotation into normal coordinates
R << tangent1, tangent2, this_normal;
auto J = R*(JA-JB); // J = [ D1; D2; n ]
auto relative_velocity = J*v; // [ tangent1dot; tangent2dot; phidot ]
if (false) {
// spring law for normal force: fA_normal = -k*phi - b*phidot
// and damping for tangential force: fA_tangent = -b*tangentdot (bounded by the friction cone)
double k = 150, b = k / 10; // todo: put these somewhere better... or make them parameters?
Vector3d fA;
fA(2) = -k * phi(i) - b * relative_velocity(2);
fA.head(2) = -std::min(b, mu*fA(2)/(relative_velocity.head(2).norm()+EPSILON)) * relative_velocity.head(2); // epsilon to avoid divide by zero
// equal and opposite: fB = -fA.
// tau = (R*JA)^T fA + (R*JB)^T fB = J^T fA
C -= J.transpose()*fA;
} else { // linear acceleration constraints (more expensive, but less tuning required for robot mass, etc)
// note: this does not work for the multi-contact case (it overly constrains the motion of the link). Perhaps if I made them inequality constraints...
static_assert(!use_multi_contact, "The acceleration contact constraints do not play well with multi-contact");
// phiddot = -2*alpha*phidot - alpha^2*phi // critically damped response
// tangential_velocity_dot = -2*alpha*tangential_velocity
double alpha = 20; // todo: put this somewhere better... or make them parameters?
Vector3d desired_relative_acceleration = -2*alpha*relative_velocity;
desired_relative_acceleration(2) += -alpha*alpha*phi(i);
// relative_acceleration = J*vdot + R*(JAdotv - JBdotv) // uses the standard dnormal/dq = 0 assumption
cout << "phi = " << phi << endl;
cout << "desired acceleration = " << desired_relative_acceleration.transpose() << endl;
// cout << "acceleration = " << (J*vdot + R*(JAdotv - JBdotv)).transpose() << endl;
prog.addContinuousVariables(3,"contact normal force");
auto JAdotv = tree->transformPointsJacobianDotTimesV(kinsol, xA.col(i).eval(), bodyA_idx[i], 0);
auto JBdotv = tree->transformPointsJacobianDotTimesV(kinsol, xB.col(i).eval(), bodyB_idx[i], 0);
prog.addLinearEqualityConstraint(J,desired_relative_acceleration - R*(JAdotv - JBdotv),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+3);
H_and_neg_JT.rightCols(3) = -J.transpose();
}
}
}
}
if (tree->getNumPositionConstraints()) {
int nc = tree->getNumPositionConstraints();
const double alpha = 5.0; // 1/time constant of position constraint satisfaction (see my latex rigid body notes)
prog.addContinuousVariables(nc,"position constraint force"); // don't actually need to use the decision variable reference that would be returned...
// then compute the constraint force
auto phi = tree->positionConstraints(kinsol);
auto J = tree->positionConstraintsJacobian(kinsol,false);
auto Jdotv = tree->positionConstraintsJacDotTimesV(kinsol);
// phiddot = -2 alpha phidot - alpha^2 phi (0 + critically damped stabilization term)
prog.addLinearEqualityConstraint(J,-(Jdotv + 2*alpha*J*v + alpha*alpha*phi),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+J.rows());
H_and_neg_JT.rightCols(J.rows()) = -J.transpose();
}
// add [H,-J^T]*[vdot;f] = -C
prog.addLinearEqualityConstraint(H_and_neg_JT,-C);
prog.solve();
// prog.printSolution();
StateVector<double> dot(nq+nv);
dot << kinsol.transformPositionDotMappingToVelocityMapping(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(nq, nq))*v, vdot.value();
return dot;
}
RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(const double& t, const RigidBodySystem::StateVector<double>& x, const RigidBodySystem::InputVector<double>& u) const {
using namespace std;
using namespace Eigen;
eigen_aligned_unordered_map<const RigidBody *, Matrix<double, 6, 1> > f_ext;
// todo: make kinematics cache once and re-use it (but have to make one per type)
auto nq = tree->num_positions;
auto nv = tree->num_velocities;
auto num_actuators = tree->actuators.size();
auto q = x.topRows(nq);
auto v = x.bottomRows(nv);
auto kinsol = tree->doKinematics(q,v);
// todo: preallocate the optimization problem and constraints, and simply update them then solve on each function eval.
// happily, this clunkier version seems fast enough for now
// the optimization framework should support this (though it has not been tested thoroughly yet)
OptimizationProblem prog;
auto const & vdot = prog.addContinuousVariables(nv,"vdot");
auto H = tree->massMatrix(kinsol);
Eigen::MatrixXd H_and_neg_JT = H;
VectorXd C = tree->dynamicsBiasTerm(kinsol,f_ext);
if (num_actuators > 0) C -= tree->B*u.topRows(num_actuators);
{ // loop through rigid body force elements
// todo: distinguish between AppliedForce and ConstraintForce
size_t u_index = 0;
for (auto const & f : force_elements) {
size_t num_inputs = f->getNumInputs();
VectorXd force_input(u.middleRows(u_index,num_inputs));
C -= f->output(t,force_input,kinsol);
u_index += num_inputs;
}
}
{ // apply joint limit forces
for (auto const& b : tree->bodies) {
if (!b->hasParent()) continue;
auto const& joint = b->getJoint();
if (joint.getNumPositions()==1 && joint.getNumVelocities()==1) { // taking advantage of only single-axis joints having joint limits makes things easier/faster here
double qmin = joint.getJointLimitMin()(0), qmax = joint.getJointLimitMax()(0);
// tau = k*(qlimit-q) - b(qdot)
if (q(b->position_num_start)<qmin)
C(b->velocity_num_start) -= penetration_stiffness*(qmin-q(b->position_num_start)) - penetration_damping*v(b->velocity_num_start);
else if (q(b->position_num_start)>qmax)
C(b->velocity_num_start) -= penetration_stiffness*(qmax-q(b->position_num_start)) - penetration_damping*v(b->velocity_num_start);
}
}
}
{ // apply contact forces
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
if (use_multi_contact)
tree->potentialCollisions(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
else
tree->collisionDetect(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
for (int i=0; i<phi.rows(); i++) {
if (phi(i)<0.0) { // then i have contact
// todo: move this entire block to a shared an updated "contactJacobian" method in RigidBodyTree
auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i], 0, false);
auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i], 0, false);
Vector3d this_normal = normal.col(i);
// compute the surface tangent basis
Vector3d tangent1;
if (1.0 - this_normal(2) < EPSILON) { // handle the unit-normal case (since it's unit length, just check z)
tangent1 << 1.0, 0.0, 0.0;
} else if (1 + this_normal(2) < EPSILON) {
tangent1 << -1.0, 0.0, 0.0; //same for the reflected case
} else {// now the general case
tangent1 << this_normal(1), -this_normal(0), 0.0;
tangent1 /= sqrt(this_normal(1)*this_normal(1) + this_normal(0)*this_normal(0));
}
Vector3d tangent2 = tangent1.cross(this_normal);
Matrix3d R; // rotation into normal coordinates
R << tangent1, tangent2, this_normal;
auto J = R*(JA-JB); // J = [ D1; D2; n ]
auto relative_velocity = J*v; // [ tangent1dot; tangent2dot; phidot ]
if (true) {
// spring law for normal force: fA_normal = -k*phi - b*phidot
// and damping for tangential force: fA_tangent = -b*tangentdot (bounded by the friction cone)
Vector3d fA;
fA(2) = -penetration_stiffness * phi(i) - penetration_damping * relative_velocity(2);
fA.head(2) = -std::min(penetration_damping, friction_coefficient*fA(2)/(relative_velocity.head(2).norm()+EPSILON)) * relative_velocity.head(2); // epsilon to avoid divide by zero
// equal and opposite: fB = -fA.
// tau = (R*JA)^T fA + (R*JB)^T fB = J^T fA
C -= J.transpose()*fA;
} else { // linear acceleration constraints (more expensive, but less tuning required for robot mass, etc)
// note: this does not work for the multi-contact case (it overly constrains the motion of the link). Perhaps if I made them inequality constraints...
assert(!use_multi_contact && "The acceleration contact constraints do not play well with multi-contact");
// phiddot = -2*alpha*phidot - alpha^2*phi // critically damped response
// tangential_velocity_dot = -2*alpha*tangential_velocity
double alpha = 20; // todo: put this somewhere better... or make them parameters?
Vector3d desired_relative_acceleration = -2*alpha*relative_velocity;
desired_relative_acceleration(2) += -alpha*alpha*phi(i);
// relative_acceleration = J*vdot + R*(JAdotv - JBdotv) // uses the standard dnormal/dq = 0 assumption
prog.addContinuousVariables(3,"contact normal force");
auto JAdotv = tree->transformPointsJacobianDotTimesV(kinsol, xA.col(i).eval(), bodyA_idx[i], 0);
auto JBdotv = tree->transformPointsJacobianDotTimesV(kinsol, xB.col(i).eval(), bodyB_idx[i], 0);
prog.addLinearEqualityConstraint(J,desired_relative_acceleration - R*(JAdotv - JBdotv),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+3);
H_and_neg_JT.rightCols(3) = -J.transpose();
}
}
}
}
if (tree->getNumPositionConstraints()) {
int nc = tree->getNumPositionConstraints();
const double alpha = 5.0; // 1/time constant of position constraint satisfaction (see my latex rigid body notes)
prog.addContinuousVariables(nc,"position constraint force"); // don't actually need to use the decision variable reference that would be returned...
// then compute the constraint force
auto phi = tree->positionConstraints(kinsol);
auto J = tree->positionConstraintsJacobian(kinsol,false);
auto Jdotv = tree->positionConstraintsJacDotTimesV(kinsol);
// phiddot = -2 alpha phidot - alpha^2 phi (0 + critically damped stabilization term)
prog.addLinearEqualityConstraint(J,-(Jdotv + 2*alpha*J*v + alpha*alpha*phi),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+J.rows());
H_and_neg_JT.rightCols(J.rows()) = -J.transpose();
}
// add [H,-J^T]*[vdot;f] = -C
prog.addLinearEqualityConstraint(H_and_neg_JT,-C);
prog.solve();
// prog.printSolution();
StateVector<double> dot(nq+nv);
dot << kinsol.transformPositionDotMappingToVelocityMapping(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(nq, nq))*v, vdot.value();
return dot;
}
