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