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