jspace::Status OpspacePlanarController::
init(jspace::Model const & model)
{
jspace::Status status(jgoal_controller_.init(model));
if ( ! status) {
return status;
}
taoDNode * q1_node(model.getNodeByName(q1_name_));
if ( ! q1_node) {
status.ok = false;
status.errstr = "invalid q1_name, no node called `" + q1_name_ + "'";
return status;
}
q1_index_ = q1_node->getID();
taoDNode * q2_node(model.getNodeByName(q2_name_));
if ( ! q2_node) {
status.ok = false;
status.errstr = "invalid q2_name, no node called `" + q2_name_ + "'";
return status;
}
q2_index_ = q2_node->getID();
ndof_ = model.getNDOF();
return status;
}
virtual jspace::Status update(jspace::Model const & model)
{
//////////////////////////////////////////////////
// Update the state of our task. Again, this is not critical
// here, but important later when we want to integrate several
// operational space tasks into a hierarchy.
actual_ = model.getState().position_;
//////////////////////////////////////////////////
// Compute PD control torques and store them in command_ for
// later retrieval. If enabled, add the estimated effect of
// gravity in order to make the robot behave as if was
// weightless.
command_ = kp_ * (goal_ - actual_) - kd_ * model.getState().velocity_;
if (enable_gravity_compensation_) {
jspace::Vector gg;
if ( ! model.getGravity(gg)) {
return jspace::Status(false, "failed to retrieve gravity torque");
}
command_ += gg;
}
jspace::Status ok;
return ok;
}
jspace::Status OpspacePlanarController::
latch(jspace::Model const & model)
{
jspace::Status status(jgoal_controller_.latch(model));
if ( ! status) {
return status;
}
jspace::State const & state(model.getState());
double q1(state.position_[q1_index_]);
double q2(state.position_[q2_index_]);
if (q1_inverted_) {
q1 = -q1;
}
if (q2_inverted_) {
q2 = -q2;
}
double const c1(cos(q1));
double const s1(sin(q1));
double const c12(cos(q1+q2));
double const s12(sin(q1+q2));
op_goal_[0] = l1_length_ * c1 + l2_length_ * c12;
op_goal_[1] = l1_length_ * s1 + l2_length_ * s12;
return status;
}
// used as fall-back controller
static void StepFloat(jspace::Model const & model, jspace::Vector & tau)
{
// this simplistic float mode can only handle a signle scalar kd
// value...
if (gain_kd.size() != 1) {
tau = jspace::Vector::Zero(7);
endme(269);
return;
}
jspace::Vector gravity_torque;
model.getGravity(gravity_torque);
jspace::Matrix massInertia;
model.getMassInertia(massInertia);
tau = gravity_torque - gain_kd[0] * massInertia * model.getState().velocity_;
}
static void draw_jspace(jspace::Model const & model)
{
for (size_t ii(0); ii < model.getNDOF(); ++ii) {
taoDNode * node(model.getNode(ii));
if ( ! node) {
continue;
}
jspace::Transform gframe;
if (model.getGlobalFrame(node, gframe)) {
draw_transform(gframe);
}
if (model.computeGlobalCOMFrame(node, gframe)) {
glColor3d(0.8, 0.6, 0.8);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glTranslated(gframe.translation().x(), gframe.translation().y(), gframe.translation().z());
glutSolidSphere(gfx_param.com_radius, 20, 16);
glPopMatrix();
}
}
}
virtual jspace::Status init(jspace::Model const & model)
{
//////////////////////////////////////////////////
// The Jacobian is not really required for pure jspace control,
// but will become very important for operational-space control.
jacobian_ = jspace::Matrix::Identity(model.getNDOF(), model.getNDOF());
//////////////////////////////////////////////////
// Initialize our PD parameters.
// kp_ = 80.0; // BIG only for unit robot!
// kd_ = 40.0;
// kp_ = 2.0; // BIG only for unit robot!
// kd_ = 0.03;
kp_ = 0.7; // BIG only for unit robot!
kd_ = 0.04;
//////////////////////////////////////////////////
// Initialize our goal to the current configuration.
goal_ = model.getState().position_;
//j//
goal_[0] = -28.0 * M_PI / 180.0;
goal_[1] = 80.0 * M_PI / 180.0;
goal_[2] = 15.0 * M_PI / 180.0;
goal_[3] = 50.0 * M_PI / 180.0;
goal_[4] = 0.0 * M_PI / 180.0;
goal_[4] = 0.0 * M_PI / 180.0;
goal_[4] = 0.0 * M_PI / 180.0;
//
//////////////////////////////////////////////////
// No initialization problems to report: the default constructor
// of jspace::Status yields an instance that signifies success.
jspace::Status ok;
return ok;
}
jspace::Status OpspacePlanarController::
computeCommand(jspace::Model const & model, jspace::Vector & tau)
{
// First find out what the joint-space controller would do, later
// overwrite just the two entries that make up our "planar" robot.
jspace::Status status(jgoal_controller_.computeCommand(model, tau));
if ( ! status) {
return status;
}
if (tau.size() != ndof_) {
status.ok = false;
status.errstr = "whoa, encapsulated jgoal_controller has a different NDOF???";
return status;
}
jspace::State const & state(model.getState());
double q1(state.position_[q1_index_]);
double q2(state.position_[q2_index_]);
if (q1_inverted_) {
q1 = -q1;
}
if (q2_inverted_) {
q2 = -q2;
}
double const c1(cos(q1));
double const s1(sin(q1));
double const c12(cos(q1+q2));
double const s12(sin(q1+q2));
op_position_[0] = l1_length_ * c1 + l2_length_ * c12;
op_position_[1] = l1_length_ * s1 + l2_length_ * s12;
jacobian_.coeffRef(0, 0) = - l1_length_ * s1 - l2_length_ * s12;
jacobian_.coeffRef(0, 1) = - l2_length_ * s12;
jacobian_.coeffRef(1, 0) = l1_length_ * c1 + l2_length_ * c12;
jacobian_.coeffRef(1, 1) = l2_length_ * c12;
// op_velocity_ is only really stored as field for debugging...
jspace::Vector qvel(2); // will probably need some smoothing on qvel
qvel[0] = state.velocity_[q1_index_];
qvel[1] = state.velocity_[q2_index_];
op_velocity_ = jacobian_ * qvel;
// op_fstar_ is only really stored as field for debugging...
if (0 > op_kp_by_kd_) {
// fall back on pure proportional control
op_fstar_ = - op_kp_ * (op_position_ - op_goal_);
}
else {
jspace::Vector vdes(2);
if (op_vmax_ < 1e-3) {
vdes = jspace::Vector::Zero(2);
}
else {
vdes = op_kp_by_kd_ * (op_goal_ - op_position_);
double const vdes_norm(vdes.norm());
if (vdes_norm > op_vmax_) {
vdes *= op_vmax_ / vdes_norm;
}
}
op_fstar_ = - op_kd_ * (op_velocity_ - vdes);
}
// taustar is the 2DOF torque that we want to send to the planar
// robot, so we use its values to overwrite what the joint-space
// controller would have wanted to do with those two.
jspace::Vector taustar(jacobian_.transpose() * op_fstar_);
if (q1_inverted_) {
tau[q1_index_] = -taustar[0];
}
else {
tau[q1_index_] = taustar[0];
}
if (q2_inverted_) {
tau[q2_index_] = -taustar[1];
}
else {
tau[q2_index_] = taustar[1];
}
return status;
}
static void StepTaskPosture(jspace::Model const & model, jspace::Vector & tau)
{
static taoDNode * right_hand(0);
if ( ! right_hand) {
right_hand = model.getNodeByName("right-hand");
if ( ! right_hand) {
tau = jspace::Vector::Zero(7);
endme(378);
return;
}
}
//////////////////////////////////////////////////
// task
jspace::Transform eepos;
model.computeGlobalFrame(right_hand, 0.0, -0.15, 0.0, eepos);
jspace::Matrix Jfull;
model.computeJacobian(right_hand,
eepos.translation()[0],
eepos.translation()[1],
eepos.translation()[2],
Jfull);
jspace::Matrix Jx(Jfull.block(0, 0, 3, 7));
jspace::Matrix invA;
model.getInverseMassInertia(invA);
jspace::Matrix invLambda_t(Jx * invA * Jx.transpose());
Eigen::SVD<jspace::Matrix> svdLambda_t(invLambda_t);
svdLambda_t.sort();
int const nrows_t(svdLambda_t.singularValues().rows());
jspace::Matrix Sinv_t;
Sinv_t = jspace::Matrix::Zero(nrows_t, nrows_t);
for (int ii(0); ii < nrows_t; ++ii) {
if (svdLambda_t.singularValues().coeff(ii) > 1e-3) {
Sinv_t.coeffRef(ii, ii) = 1.0 / svdLambda_t.singularValues().coeff(ii);
}
}
jspace::Matrix Lambda_t(svdLambda_t.matrixU() * Sinv_t * svdLambda_t.matrixU().transpose());
dbg_invLambda_t = invLambda_t;
dbg_Lambda_t = Lambda_t;
static jspace::Vector eegoal0;
if (0 == eegoal0.size()) {
if (goal.rows() < 3) {
tau = jspace::Vector::Zero(7);
endme(256);
return;
}
eegoal0 = goal.block(0, 0, 3, 1);
}
struct timeval now;
gettimeofday(&now, 0);
double const alpha((1.0 * now.tv_sec + 1.0e-6 * now.tv_usec) * M_PI / 2.0);
jspace::Vector eegoal(eegoal0);
eegoal.coeffRef(1) += 0.2 * cos(alpha);
eegoal.coeffRef(2) += 0.2 * sin(alpha);
jspace::Vector poserror(eepos.translation() - eegoal);
jspace::Vector g;
model.getGravity(g);
static jspace::Vector eegain_kp, eegain_kd;
if (0 == eegain_kp.size()) {
if (3 <= gain_kp.size()) {
eegain_kp = gain_kp.block(0, 0, 3, 1);
}
else {
eegain_kp = gain_kp[0] * jspace::Vector::Ones(3);
}
if (3 <= gain_kd.size()) {
eegain_kd = gain_kd.block(0, 0, 3, 1);
}
else {
eegain_kd = gain_kd[0] * jspace::Vector::Ones(3);
}
}
jspace::Vector
tau_task(Jx.transpose() * (-Lambda_t)
* ( eegain_kp.cwise() * poserror
+ eegain_kd.cwise() * Jx * model.getState().velocity_));
//////////////////////////////////////////////////
// posture
jspace::Matrix Jbar(invA * Jx.transpose() * Lambda_t);
jspace::Matrix nullspace(jspace::Matrix::Identity(7, 7) - Jbar * Jx);
static jspace::Vector goalposture;
if (7 != goalposture.size()) {
if (goal.size() >= 10) {
goalposture = goal.block(3, 0, 7, 1);
goalposture *= M_PI / 180;
}
else {
goalposture = jspace::Vector::Zero(7);
goalposture[1] = 30 * M_PI / 180; // shoulder "sideways" 30 degrees
goalposture[3] = 60 * M_PI / 180; // ellbow at 60 degrees
}
}
jspace::Matrix invLambda_p(nullspace * invA);
Eigen::SVD<jspace::Matrix> svdLambda_p(invLambda_p);
svdLambda_p.sort();
int const nrows_p(svdLambda_p.singularValues().rows());
jspace::Matrix Sinv_p;
Sinv_p = jspace::Matrix::Zero(nrows_p, nrows_p);
for (int ii(0); ii < nrows_p; ++ii) {
if (svdLambda_p.singularValues().coeff(ii) > 1e-3) {
Sinv_p.coeffRef(ii, ii) = 1.0 / svdLambda_p.singularValues().coeff(ii);
}
}
jspace::Matrix Lambda_p(svdLambda_p.matrixU() * Sinv_p * svdLambda_p.matrixU().transpose());
dbg_invLambda_p = invLambda_p;
dbg_Lambda_p = Lambda_p;
static jspace::Vector posturegain_kp, posturegain_kd;
if (0 == posturegain_kp.size()) {
if (10 <= gain_kp.size()) {
posturegain_kp = gain_kp.block(3, 0, 7, 1);
}
else {
if (1 == gain_kp.size()) {
posturegain_kp = gain_kp[0] * jspace::Vector::Ones(7);
}
else {
posturegain_kp = gain_kp[1] * jspace::Vector::Ones(7);
}
}
if (10 <= gain_kd.size()) {
posturegain_kd = gain_kd.block(3, 0, 7, 1);
}
else {
if (1 == gain_kd.size()) {
posturegain_kd = gain_kd[0] * jspace::Vector::Ones(7);
}
else {
posturegain_kd = gain_kd[1] * jspace::Vector::Ones(7);
}
}
}
jspace::Vector tau_posture(nullspace.transpose() * (-Lambda_p)
* (posturegain_kp.cwise() * (model.getState().position_ - goalposture)
+ posturegain_kd.cwise() * model.getState().velocity_));
//////////////////////////////////////////////////
// sum it up...
tau = tau_task + tau_posture + g;
}