bool Contains(vec_t point) { vec_t upper_limit = center_ + half_space_, lower_limit = center_ - half_space_; for (int i = 0;i < D; ++i) if (lower_limit(i) > point(i) || upper_limit(i) < point(i)) return false; return true; }
void spin() { // initialize random seed srand(time(NULL)); // dynamics model boost::shared_ptr<DynamicModel > dyn_model( new DynModelPlanar5() ); std::string robot_description_str; std::string robot_semantic_description_str; nh_.getParam("/robot_description", robot_description_str); nh_.getParam("/robot_semantic_description", robot_semantic_description_str); // // collision model // boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str); col_model->parseSRDF(robot_semantic_description_str); col_model->generateCollisionPairs(); // external collision objects - part of virtual link connected to the base link self_collision::Link::VecPtrCollision col_array; col_array.push_back( self_collision::createCollisionCapsule(0.2, 0.3, KDL::Frame(KDL::Rotation::RotX(90.0/180.0*PI), KDL::Vector(1, 0.5, 0))) ); if (!col_model->addLink("env_link", "base", col_array)) { ROS_ERROR("ERROR: could not add external collision objects to the collision model"); return; } col_model->generateCollisionPairs(); // // robot state // std::vector<std::string > joint_names; joint_names.push_back("0_joint"); joint_names.push_back("1_joint"); joint_names.push_back("2_joint"); joint_names.push_back("3_joint"); joint_names.push_back("4_joint"); int ndof = joint_names.size(); Eigen::VectorXd q, dq, ddq, torque; q.resize( ndof ); dq.resize( ndof ); ddq.resize( ndof ); torque.resize( ndof ); for (int q_idx = 0; q_idx < ndof; q_idx++) { q[q_idx] = -0.1; dq[q_idx] = 0.0; ddq[q_idx] = 0.0; torque[q_idx] = 0.0; } std::string effector_name = "effector"; int effector_idx = col_model->getLinkIndex(effector_name); // // kinematic model // boost::shared_ptr<KinematicModel> kin_model( new KinematicModel(robot_description_str, joint_names) ); KinematicModel::Jacobian J_r_HAND_6, J_r_HAND; J_r_HAND_6.resize(6, ndof); J_r_HAND.resize(3, ndof); std::vector<KDL::Frame > links_fk(col_model->getLinksCount()); // joint limits Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof); int q_idx = 0; for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) { lower_limit[q_idx] = kin_model->getLowerLimit(q_idx); upper_limit[q_idx] = kin_model->getUpperLimit(q_idx); limit_range[q_idx] = 10.0/180.0*PI; max_trq[q_idx] = 10.0; } Eigen::VectorXd max_q(ndof); max_q(0) = 10.0/180.0*PI; max_q(1) = 20.0/180.0*PI; max_q(2) = 20.0/180.0*PI; max_q(3) = 30.0/180.0*PI; max_q(4) = 40.0/180.0*PI; boost::shared_ptr<DynamicsSimulatorHandPose> sim( new DynamicsSimulatorHandPose(ndof, 6, effector_name, col_model, kin_model, dyn_model, joint_names, max_q) ); /* // // Tasks declaration // Task_JLC task_JLC(lower_limit, upper_limit, limit_range, max_trq); Task_WCC task_WCC(ndof, 3, 4); double activation_dist = 0.05; Task_COL task_COL(ndof, activation_dist, 10.0, kin_model, col_model); Task_HAND task_HAND(ndof, 3); */ /* while (ros::ok()) { // // wrist collision constraint // Eigen::VectorXd torque_WCC(ndof); Eigen::MatrixXd N_WCC(ndof, ndof); task_WCC.compute(q, dq, dyn_model->getM(), dyn_model->getInvM(), torque_WCC, N_WCC, markers_pub_); markers_pub_.publish(); ros::spinOnce(); char ch = getchar(); if (ch == 'a') { q(3) -= 0.1; } else if (ch == 'd') { q(3) += 0.1; } else if (ch == 's') { q(4) -= 0.1; } else if (ch == 'w') { q(4) += 0.1; } } return; */ // loop variables ros::Time last_time = ros::Time::now(); KDL::Frame r_HAND_target; int loop_counter = 10000; ros::Rate loop_rate(100); while (true) { generatePossiblePose(r_HAND_target, q, ndof, effector_name, col_model, kin_model); sim->setState(q, dq, ddq); sim->setTarget(r_HAND_target); sim->oneStep(); if (!sim->inCollision()) { break; } } while (ros::ok()) { if (loop_counter > 500) { Eigen::VectorXd q_tmp(ndof); generatePossiblePose(r_HAND_target, q_tmp, ndof, effector_name, col_model, kin_model); sim->setTarget(r_HAND_target); publishTransform(br, r_HAND_target, "effector_dest", "base"); loop_counter = 0; } loop_counter += 1; sim->oneStep(&markers_pub_, 3000); /* if (sim->inCollision() && !collision_in_prev_step) { collision_in_prev_step = true; std::cout << "collision begin" << std::endl; } else if (!sim->inCollision() && collision_in_prev_step) { collision_in_prev_step = false; std::cout << "collision end" << std::endl; } */ sim->getState(q, dq, ddq); // publish markers and robot state with limited rate ros::Duration time_elapsed = ros::Time::now() - last_time; if (time_elapsed.toSec() > 0.05) { // calculate forward kinematics for all links for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) { kin_model->calculateFk(links_fk[l_idx], col_model->getLinkName(l_idx), q); } publishJointState(joint_state_pub_, q, joint_names); int m_id = 0; m_id = addRobotModelVis(markers_pub_, m_id, col_model, links_fk); markers_pub_.publish(); ros::Time last_time = ros::Time::now(); } ros::spinOnce(); loop_rate.sleep(); } }
DynamicsSimulatorHandPose::DynamicsSimulatorHandPose(int ndof, int ndim, const std::string &effector_name, const boost::shared_ptr<self_collision::CollisionModel> &col_model, const boost::shared_ptr<KinematicModel> &kin_model, const boost::shared_ptr<DynamicModel > &dyn_model, const std::vector<std::string > &joint_names, const Eigen::VectorXd &q_eq, const Eigen::VectorXd &max_q, boost::function<KDL::Twist(const KDL::Frame &x, const KDL::Frame &y)> pose_diff_function) : ndof_(ndof), ndim_(ndim), effector_name_(effector_name), q_( Eigen::VectorXd::Zero(ndof_) ), dq_( Eigen::VectorXd::Zero(ndof_) ), ddq_( Eigen::VectorXd::Zero(ndof_) ), torque_( Eigen::VectorXd::Zero(ndof_) ), col_model_(col_model), kin_model_(kin_model), dyn_model_(dyn_model), links_fk_(col_model->getLinksCount()), activation_dist_(0.05), torque_JLC_( Eigen::VectorXd::Zero(ndof_) ), N_JLC_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), torque_WCCr_( Eigen::VectorXd::Zero(ndof_) ), N_WCCr_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), torque_WCCl_( Eigen::VectorXd::Zero(ndof_) ), N_WCCl_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), torque_COL_( Eigen::VectorXd::Zero(ndof_) ), N_COL_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), torque_HAND_( Eigen::VectorXd::Zero(ndof_) ), N_HAND_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), torque_JOINT_( Eigen::VectorXd::Zero(ndof_) ), N_JOINT_( Eigen::MatrixXd::Identity(ndof_, ndof_) ), Kc_(ndim_), Dxi_(ndim_), Kc_JOINT_(ndof_), Dxi_JOINT_(ndof_), r_HAND_diff_(ndim_), in_collision_(false), max_q_(max_q), q_eq_(q_eq), pose_diff_function_(pose_diff_function) { for (int q_idx = 0; q_idx < ndof; q_idx++) { Kc_JOINT_[q_idx] = 1.0; Dxi_JOINT_[q_idx] = 0.1; } double Kc_lin = 200.0; double Kc_rot = 20.0; if (ndim_ == 3) { Kc_[0] = Kc_lin; Kc_[1] = Kc_lin; Kc_[2] = Kc_rot; } else { for (int dim_idx = 0; dim_idx < 3; dim_idx++) { Kc_[dim_idx] = Kc_lin; Kc_[dim_idx+3] = Kc_rot; } } for (int dim_idx = 0; dim_idx < ndim_; dim_idx++) { Dxi_[dim_idx] = 0.7; } effector_idx_ = col_model->getLinkIndex(effector_name_); int wcc_r_q5_idx=-1, wcc_r_q6_idx=-1; int wcc_l_q5_idx=-1, wcc_l_q6_idx=-1; // joint limits Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof); int q_idx = 0; for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) { if ( (*name_it) == "right_arm_5_joint" ) { wcc_r_q5_idx = q_idx; } else if ( (*name_it) == "right_arm_6_joint" ) { wcc_r_q6_idx = q_idx; } else if ( (*name_it) == "left_arm_5_joint" ) { wcc_l_q5_idx = q_idx; } else if ( (*name_it) == "left_arm_6_joint" ) { wcc_l_q6_idx = q_idx; } lower_limit[q_idx] = kin_model->getLowerLimit(q_idx); upper_limit[q_idx] = kin_model->getUpperLimit(q_idx); limit_range[q_idx] = 10.0 / 180.0 * 3.141592653589793; max_trq[q_idx] = 10.0; } if (ndim_ != 3 && ndim_ != 6) { std::cout << "ERROR: DynamicsSimulatorHandPose works for 3 or 6 dimensions" << std::endl; } std::set<int > jlc_excluded_q_idx; if (wcc_r_q5_idx != -1 && wcc_r_q6_idx != -1) { jlc_excluded_q_idx.insert(wcc_r_q5_idx); jlc_excluded_q_idx.insert(wcc_r_q6_idx); task_WCCr_.reset( new Task_WCC(ndof, wcc_r_q5_idx, wcc_r_q6_idx, false) ); } if (wcc_l_q5_idx != -1 && wcc_l_q6_idx != -1) { jlc_excluded_q_idx.insert(wcc_l_q5_idx); jlc_excluded_q_idx.insert(wcc_l_q6_idx); task_WCCl_.reset( new Task_WCC(ndof, wcc_l_q5_idx, wcc_l_q6_idx, true) ); } task_JLC_.reset( new Task_JLC(lower_limit, upper_limit, limit_range, max_trq, jlc_excluded_q_idx) ); task_COL_.reset( new Task_COL(ndof_, activation_dist_, 80.0, kin_model_, col_model_) ); task_HAND_.reset( new Task_HAND(ndof_, ndim_) ); J_r_HAND_6_.resize(6, ndof_); J_r_HAND_.resize(ndim_, ndof_); task_JOINT_.reset( new Task_JOINT(ndof_) ); }