/* Pair : row,column */ int lee_algo(Pair* source, Pair* target){ int i, j, v1, v2; Pair p, v, step[4] = {{1,0},{0,1},{-1,0},{0,-1}}; /* initialize all cells like unvisited */ for(i=0; i<sz.r; ++i){ for(j=0; j<sz.c; ++j){ W[i][j] = -1; } } q_clear(); p_set(W,source,0); q_push(source); while(!q_empty()){ p = q_pop(); for(i=0; i<4; ++i){ v = p_sum(step+i,&p); v1 = p_get(Z,&p); v2 = p_get(Z,&v); #ifdef DEBUG printf("try: %d,%d\n",v.r,v.c); #endif if(p_in(&v) && p_get(W,&v)==-1 && (v1-v2 <= 3) && (v1-v2 >= -1) && can_go(&v)){ #ifdef DEBUG printf("from: %d,%d go: %d,%d\n",p.r,p.c,v.r,v.c); #endif p_set(W,&v,p_get(W,&p)+1); q_push(&v); if(p_eq(&v,target)) break; } } if(p_eq(&v,target)) break; } #ifdef DEBUG printf("q: %d\n",q_empty()); #endif return p_get(W,target); }
int hadlock_algo(Pair* source, Pair* target){ int i, j, v1, v2, res; Pair p, v, step[4] = {{1,0},{0,1},{-1,0},{0,-1}}; /* initialize all cells like unvisited */ for(i=0; i<sz.r; ++i){ for(j=0; j<sz.c; ++j){ W[i][j] = -1; } } q_clear(); p_set(W,source,0); q_push(source); while(!q_empty()){ p = q_pop(); for(i=0; i<4; ++i){ v = p_sum(step+i,&p); v1 = p_get(Z,&p); v2 = p_get(Z,&v); if(p_in(&v) && (p_get(W,&v)==-1 || (p_get(W,&v)>p_get(W,&p))) && (v1-v2 <= 3) && (v1-v2 >= -1) && can_go(&v)){ if(p_mhtn_norm(&v,target) < p_mhtn_norm(&p,target)){ p_set(W,&v,p_get(W,&p)); q_push_back(&v); }else{ /* > */ /*if(!(p_get(W,&v)!=-1 || (p_get(W,&v)==p_get(W,&p)+1))){ */ p_set(W,&v,p_get(W,&p)+1); q_push_front(&v); } if(p_eq(&v,target)){ break; } } } if(p_eq(&v,target)) break; } res = p_get(W,target); if(res != -1) res = 2*res+p_mhtn_norm(source,target); return res; }
int main(int argc, char **argv) { ros::init(argc, argv, "leg_kinematics"); KDL::Tree tree; KDL::Chain chain; ros::NodeHandle node; KDL::ChainFkSolverPos_recursive* fk_solver; KDL::HP_ChainIkSolverPos_NR_JL *ik_solver_pos; KDL::ChainIkSolverVel_pinv* ik_solver_vel; std::string robot_desc_string; node.param("robot_description", robot_desc_string, std::string("robot_description")); if (!kdl_parser::treeFromString(robot_desc_string, tree)) { ROS_ERROR("Failed to construct kdl tree"); return false; } if (!tree.getChain("base_link", "tibia_foot_r1", chain)) { ROS_ERROR("Failed to construct kdl chain"); return false; } ROS_INFO("Construct kdl chain"); // unsigned int nj = chain.getNrOfJoints(); // unsigned int js = chain.getNrOfSegments(); // std_msgs::String msg; // std::stringstream ss; // ss << "# J: " << nj << " # S: " << js; // msg.data = ss.str(); // ROS_INFO("Construct kdl tree"); // ROS_INFO("%s", msg.data.c_str()) ; fk_solver = new KDL::ChainFkSolverPos_recursive(chain); ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain); // Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6); // matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0; // ik_solver_vel -> setWeightTS(matrix_Mx); KDL::JntArray joint_min(chain.getNrOfJoints()); KDL::JntArray joint_max(chain.getNrOfJoints()); joint_min(0) = -1.57; joint_min(1) = -1.57; joint_min(2) = -1.57; joint_max (0) = 1.57; joint_max (1) = 1.57; joint_max (2) = 1.57; ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, 20, 0.0001); KDL::JntArray q_init(chain.getNrOfJoints()); q_init (0) = 0; q_init (1) = 0; q_init (2) = 0; KDL::JntArray q_out(chain.getNrOfJoints()); // KDL::Segment my_seg = chain.getSegment(3); // KDL::Frame my_fr; // // if (fk_solver.JntToCart(q_init, my_fr) >= 0){ // // std_msgs::String msg; // std::stringstream ss; // ss << "# 0: " << my_fr.p[0] << " # 1: " << my_fr.p[1] << " # 2: " << my_fr.p[2]; // msg.data = ss.str(); // ROS_INFO("Construct kdl tree"); // ROS_INFO("%s", msg.data.c_str()) ; // } double x = 0.16509, y = -0.18567, z = 0.053603; //0.16509; -0.18567; 0.053603 KDL::Frame p_in (KDL::Vector(0.12291, -0.085841, -0.16766)); int ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out); if (ik_valid >= 0) { ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2)); } else { ROS_ERROR("IK not found"); } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "leg_kinematics_pub"); ros::NodeHandle node; KDL::Tree tree; KDL::Chain chain; KDL::ChainFkSolverPos_recursive* fk_solver; KDL::HP_ChainIkSolverPos_NR_JL* ik_solver_pos; KDL::ChainIkSolverVel_pinv* ik_solver_vel; ros::Publisher leg_msg_pub = node.advertise<crab_msgs::LegJointsState>("leg_states", 1); ros::Publisher joint_msg_pub = node.advertise<sensor_msgs::JointState>("leg_joints_states", 1); ros::Rate loop_rate(30); std::string robot_desc_string; node.param("robot_description", robot_desc_string, std::string("robot_description")); if (!kdl_parser::treeFromString(robot_desc_string, tree)){ ROS_ERROR("Failed to construct kdl tree"); return false; } if (!tree.getChain("base_link", "tibia_foot_r1", chain)) { ROS_ERROR("Failed to construct kdl chain"); return false; } ROS_INFO("Construct kdl chain"); fk_solver = new KDL::ChainFkSolverPos_recursive(chain); ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain); // Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6); // matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0; // ik_solver_vel -> setWeightTS(matrix_Mx); KDL::JntArray joint_min(chain.getNrOfJoints()); KDL::JntArray joint_max(chain.getNrOfJoints()); joint_min(0) = -1.57; joint_min(1) = -1.57; joint_min(2) = -1.57; joint_max (0) = 1.57; joint_max (1) = 1.57; joint_max (2) = 1.57; ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, 100, 0.0001); KDL::JntArray q_init(chain.getNrOfJoints()); q_init (0) = 0; q_init (1) = 0; q_init (2) = 0; KDL::JntArray q_out(chain.getNrOfJoints()); crab_msgs::LegJointsState leg_msg; sensor_msgs::JointState joint_msg; int ik_valid; double x_0 = 0.14208, y_0 = -0.14555, z_0 = -0.11245, x, y, fi; //0.14208; -0.14555; -0.11145 while (ros::ok()){ if (fi > 6.28) fi = 0; x = x_0 + 0.05 * cos(fi); y = y_0 + 0.05 * sin(fi); KDL::Frame p_in (KDL::Vector(x, y, z_0)); ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out); if (ik_valid >= 0){ ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2)); for (int i=0; i<3; i++){ leg_msg.joint[i] = q_out(i); } leg_msg_pub.publish(leg_msg); joint_msg.name.push_back("coxa_joint_r1"); joint_msg.position.push_back(q_out(0)); joint_msg.name.push_back("femur_joint_r1"); joint_msg.position.push_back(q_out(1)); joint_msg.name.push_back("tibia_joint_r1"); joint_msg.position.push_back(q_out(2)); joint_msg_pub.publish(joint_msg); joint_msg.name.clear(); joint_msg.position.clear(); } else { ROS_ERROR("IK not found"); } fi += 0.05; q_init = q_out; ros::spinOnce(); loop_rate.sleep(); } return 0; }