bool RRT::collisionFree(const Eigen::VectorXd &q_from, const KDL::Frame &x_from, const KDL::Frame &x_to, int try_idx, Eigen::VectorXd &q_to, KDL::Frame &x_to_out) const {
Eigen::VectorXd dq(ndof_), ddq(ndof_);
for (int q_idx = 0; q_idx < ndof_; q_idx++) {
dq(q_idx) = 0.0;
ddq(q_idx) = 0.0;
}
sim_->setTarget(x_to);
sim_->setState(q_from, dq, ddq);
KDL::Twist diff_target = KDL::diff(x_from, x_to, 1.0);
for (int loop_counter = 0; loop_counter < 3000; loop_counter++) {
Eigen::VectorXd q(ndof_), dq(ndof_), ddq(ndof_);
sim_->getState(q, dq, ddq);
KDL::Frame r_HAND_current;
kin_model_->calculateFk(r_HAND_current, effector_name_, q);
KDL::Twist goal_diff( KDL::diff(r_HAND_current, x_to, 1.0) );
if (goal_diff.vel.Norm() < 0.03 && goal_diff.rot.Norm() < 10.0/180.0*3.1415) {
q_to = q;
x_to_out = r_HAND_current;
return true;
}
sim_->oneStep();
}
return false;
}
// Calculate the vertices of the curve as well as the normal, up, and tangent vectors.
void calculateVectors(){
double up[3] = {0.0, 0.0, 1.0};
int index = 0;
for (int j = 0; j < numberPoints-3; j++){
for (int i = 0; i <= NUMBER_SEGMENTS; i++){
qValues[(j*NUMBER_SEGMENTS + i)] = q((double)i/NUMBER_SEGMENTS, ctrlpoints[j], ctrlpoints[j+1], ctrlpoints[j+2], ctrlpoints[j+3]);
dqValues[(j*NUMBER_SEGMENTS + i)] = dq((double)i/NUMBER_SEGMENTS, ctrlpoints[j], ctrlpoints[j+1], ctrlpoints[j+2], ctrlpoints[j+3]);
ddqValues[(j*NUMBER_SEGMENTS + i)] = ddq((double)i/NUMBER_SEGMENTS, ctrlpoints[j], ctrlpoints[j+1], ctrlpoints[j+2], ctrlpoints[j+3]);
nValues[(j*NUMBER_SEGMENTS + i)] = normalizeProduct(negationProduct(dqValues[(j*NUMBER_SEGMENTS + i)]));
uValues[(j*NUMBER_SEGMENTS + i)] = normalizeProduct(crossProduct(up, nValues[(j*NUMBER_SEGMENTS + i)]));
vValues[(j*NUMBER_SEGMENTS + i)] = normalizeProduct(crossProduct(nValues[(j*NUMBER_SEGMENTS + i)], uValues[(j*NUMBER_SEGMENTS + i)]));
}
}
for (int j = 0; j < numberPoints-3; j++){
for (int i = 0; i <= NUMBER_SEGMENTS; i++){
index = j*NUMBER_SEGMENTS + i;
// Set the three rails.
leftRail[index] = setLeftRail(qValues[index], uValues[index]);
rightRail[index] = setRightRail(qValues[index], uValues[index]);
centerRail[index] = setCenterRail(qValues[index]);
columnTopRight[index] = setColumnTopRight(leftRail[index], centerRail[index]);
columnTopLeft[index] = setColumnTopRight(rightRail[index], centerRail[index]);
}
}
}
bool RTAStar::collisionFree(const Eigen::VectorXd &q_from, const Eigen::VectorXd &dq_from, const KDL::Frame &x_from, const KDL::Frame &x_to, int try_idx, Eigen::VectorXd &q_to, Eigen::VectorXd &dq_to, KDL::Frame &x_to_out,
std::list<KDL::Frame > *path_x, std::list<Eigen::VectorXd > *path_q) const {
path_x->clear();
path_q->clear();
Eigen::VectorXd q(ndof_), dq(ndof_), ddq(ndof_);
ddq.setZero();
sim_->setTarget(x_to);
sim_->setState(q_from, dq_from, ddq);
KDL::Twist diff_target = KDL::diff(x_from, x_to, 1.0);
for (int loop_counter = 0; loop_counter < 1500; loop_counter++) {
// Eigen::VectorXd q(ndof_), dq(ndof_), ddq(ndof_);
sim_->getState(q, dq, ddq);
KDL::Frame r_HAND_current;
kin_model_->calculateFk(r_HAND_current, effector_name_, q);
KDL::Twist prev_diff;
if (path_x->empty()) {
prev_diff = KDL::diff(x_from, r_HAND_current, 1.0);
}
else {
prev_diff = KDL::diff(path_x->back(), r_HAND_current, 1.0);
}
if (prev_diff.vel.Norm() > 0.1 || prev_diff.rot.Norm() > 30.0/180.0*3.1415) {
path_x->push_back(r_HAND_current);
path_q->push_back(q);
}
KDL::Twist goal_diff( KDL::diff(r_HAND_current, x_to, 1.0) );
if (goal_diff.vel.Norm() < 0.01) {// && goal_diff.rot.Norm() < 5.0/180.0*3.1415) {
q_to = q;
dq_to = dq;
x_to_out = r_HAND_current;
return true;
}
sim_->oneStep();
if (sim_->inCollision()) {
return false;
}
}
return false;
}
TEST(TestSuite, oneDofRevoluteX)
{
const char* ffJointName = "base_footprint_joint";
jrl::dynamics::urdf::Parser parser;
CjrlHumanoidDynamicRobot* robot = parser.parse
(TEST_MODEL_DIRECTORY "/one_dof_revolute_x.urdf",
ffJointName);
CjrlJoint* root = robot->rootJoint ();
ASSERT_TRUE (root);
ASSERT_EQ (1, root->countChildJoints());
ASSERT_EQ (ffJointName, root->getName());
CjrlJoint* joint = root->childJoint (0);
ASSERT_TRUE (joint);
ASSERT_EQ ("joint1", joint->getName());
// Create robot configuration.
const int ndofs = 7;
vectorN q (ndofs);
vectorN dq (ndofs);
vectorN ddq (ndofs);
for (unsigned i = 0; i < ndofs; ++i)
q (i) = dq (i) = ddq (i) = 0.;
// Check configuration at zero.
std::cout << q << std::endl;
ASSERT_TRUE (robot->currentConfiguration(q));
ASSERT_TRUE (robot->currentVelocity(dq));
ASSERT_TRUE (robot->currentAcceleration(ddq));
ASSERT_TRUE (robot->computeForwardKinematics());
matrix4d jointPosition;
jointPosition.setIdentity ();
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_EQ(jointPosition (i, j),
root->currentTransformation () (i, j));
jointPosition.setIdentity ();
jointPosition (1, 3) = 1.;
// std::cout << "current value:" << std::endl;
// std::cout << joint->currentTransformation () << std::endl;
// std::cout << "expected value:" << std::endl;
// std::cout << jointPosition << std::endl;
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_NEAR (jointPosition (i, j),
joint->currentTransformation () (i, j),
1e-9);
// Check configuration at pi/2.
q (6) = M_PI / 2.;
std::cout << q << std::endl;
ASSERT_TRUE (robot->currentConfiguration(q));
ASSERT_TRUE (robot->currentVelocity(dq));
ASSERT_TRUE (robot->currentAcceleration(ddq));
ASSERT_TRUE (robot->computeForwardKinematics());
jointPosition.setIdentity ();
jointPosition (1, 1) = 0.;
jointPosition (1, 2) = -1.;
jointPosition (2, 1) = 1.;
jointPosition (2, 2) = 0.;
jointPosition (1, 3) = 1.;
std::cout << "current value:" << std::endl;
std::cout << joint->currentTransformation () << std::endl;
std::cout << "expected value:" << std::endl;
std::cout << jointPosition << std::endl;
for (unsigned i = 0; i < 4; ++i)
for (unsigned j = 0; j < 4; ++j)
ASSERT_NEAR (jointPosition (i, j),
joint->currentTransformation () (i, j),
1e-9);
}
int main(int argc, char *argv[])
{
// In this example we will use YARP only for parsing command line
// parameters
yarp::os::ResourceFinder rf;
rf.setVerbose(true);
rf.setDefault("name","walkmanLimbJTSRegressor");
rf.setDefault("config","config.ini");
rf.configure(argc,argv);
if (rf.check("help"))
{
yInfo() << "Options:\n\n";
yInfo() << "\t--urdf file: name of the URDF file containing"
" the model of the Walkman. \n";
return 0;
}
std::string urdf_filename = rf.findFile("urdf");
yInfo() << "Tryng to open " << urdf_filename << " as Walkman model";
/**
* The dynamics regressor generator is a class for calculating arbitrary regressor
* related to robot dynamics, for identification of dynamics parameters, such
* as inertial parameters (masses, centers of mass, inertia tensor elements).
*/
//For building the regressor generator, we need several inputs:
// * the model of the robot, given by the KDL::CoDyCo::UndirectedTree object
// * a model of the sensors of the robot, given by the KDL::CoDyCo::SensorsTree object
// (at this moment the SensorsTree supports only six-axis FT sensors, so it is not needed
// for joint torque sensors regressors)
// * a kinematic base (i.e. link for which the velocity/acceleration and the gravity are known:
// in this example we will leave the default one, i.e. the root of the robot)
// * the option for considering or not FT sensors offset in the parameter estimation
// (in this example we don't consider FT sensors, so we put this to false)
// * a list of "fake_links", for which no inertial parameters will be considered, useful for removing
// from the model "fake" links such as frames.
KDL::Tree walkman_tree;
if( !kdl_format_io::treeFromUrdfFile(urdf_filename,walkman_tree) )
{
std::cerr << " Could not parse urdf robot model" << std::endl;
return EXIT_FAILURE;
}
KDL::CoDyCo::UndirectedTree walkman_undirected_tree(walkman_tree);
KDL::CoDyCo::SensorsTree walkman_sensors_tree;
std::string kinematic_base = "";
bool consider_ft_offsets = false;
std::vector< std::string > fake_links;
KDL::CoDyCo::Regressors::DynamicRegressorGenerator jts_regressor_generator(walkman_undirected_tree,
walkman_sensors_tree,
kinematic_base,
consider_ft_offsets,
fake_links);
// We add the structure now: the list of joint torque sensors that we are considering
std::vector< std::string > jts_names;
for(int jts = 0; jts < jts_names.size(); jts++ )
{
jts_regressor_generator.addTorqueRegressorRows(jts_names[jts]);
}
// Now we can print some information about the regressor
std::cout << "Regressor information: " << std::endl;
std::cout << "Outputs: " << std::endl;
std::cout << jts_regressor_generator.getDescriptionOfOutputs() << std::endl;
std::cout << "Parameters: " << std::endl;
std::cout << jts_regressor_generator.getDescriptionOfParameters() << std::endl;
// We now have to set the robot state (in the fixed base case);
KDL::JntArray q(walkman_undirected_tree.getNrOfDOFs());
KDL::JntArray dq(walkman_undirected_tree.getNrOfDOFs());
KDL::JntArray ddq(walkman_undirected_tree.getNrOfDOFs());
// For this example we will consider all joint positions, velocities and accelerations to zero
KDL::SetToZero(q);
KDL::SetToZero(dq);
KDL::SetToZero(ddq);
KDL::Vector gravity;
gravity(2) = -9.8;
KDL::Twist gravity_twist(gravity,KDL::Vector::Zero());
jts_regressor_generator.setRobotState(q,dq,ddq,gravity_twist);
// Now we can generate the actual regressor
Eigen::MatrixXd regressor(jts_regressor_generator.getNrOfOutputs(),jts_regressor_generator.getNrOfParameters());
Eigen::VectorXd known_terms(jts_regressor_generator.getNrOfOutputs());
jts_regressor_generator.computeRegressor(regressor,known_terms);
return 0;
}
