int main(int argc, char *argv[])
{
ros::init(argc, argv, "kinematics_publisher");
ros::NodeHandle nh;
ros::Publisher joint_publisher;
ros::Publisher frame_publisher;
joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);
std::vector<std::string> names;
names.resize(2);
names.at(0) = "base_to_body";
names.at(1) = "body_to_head";
sensor_msgs::JointState my_joints;
my_joints.name = names;
my_joints.position.resize(2);
my_joints.position.at(0) = 0.2;
my_joints.position.at(1) = 0,2;
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
bool exit_value;
KDL::Tree my_tree;
urdf::Model my_model;
if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;
KDL::Chain my_chain;
my_tree.getChain("world", "webcam", my_chain);
std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;
KDL::ChainFkSolverPos_recursive fksolver(my_chain);
KDL::JntArray q(my_chain.getNrOfJoints());
q(0) = my_joints.position.at(0);
q(1) = my_joints.position.at(1);
KDL::Frame webcam;
fksolver.JntToCart(q,webcam);
geometry_msgs::Pose webcam_pose;
tf::poseKDLToMsg(webcam, webcam_pose);
geometry_msgs::PoseStamped webcam_stamped;
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.frame_id = "world";
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
KDL::ChainJntToJacSolver jacobian_solver(my_chain);
KDL::Jacobian J;
J.resize(2);
jacobian_solver.JntToJac(q, J);
std::cout << J.data << std::endl;
webcam.M.DoRotY(-20*KDL::deg2rad);
tf::poseKDLToMsg(webcam, webcam_pose);
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
std::cin.get();
KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
KDL::JntArray q_sol(2);
ik_solver_pos.CartToJnt(q, webcam,q_sol);
my_joints.position.at(0) = q_sol(0);
my_joints.position.at(1) = q_sol(1);
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
KDL::Vector g (0.0,0.0,-9.81);
KDL::ChainDynParam my_chain_params(my_chain, g);
KDL::ChainIdSolver_RNE id_solver(my_chain, g);
KDL::JntArray G(2);
my_chain_params.JntToGravity(q_sol,G);
KDL::JntSpaceInertiaMatrix H(2);
my_chain_params.JntToMass(q_sol, H);
KDL::JntArray C(2);
KDL::JntArray q_dot(2);
q_dot(0) = 0.02;
q_dot(1) = 0.04;
my_chain_params.JntToCoriolis(q_sol,q_dot, C);
std::cout << "g(q): " <<G.data << std::endl;
std::cout << "M(q): " <<H.data << std::endl;
std::cout << "C(q,q_dot): " <<C.data << std::endl;
KDL::JntArray q_dotdot(2);
q_dotdot(0) = -0.02;
q_dotdot(1) = 0.02;
KDL::Wrenches f(4);
KDL::JntArray tau(2);
id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
std::cout << "Tau: " <<tau.data << std::endl;
std::cin.get();
moveit::planning_interface::MoveGroup my_group("webcam_robot");
my_group.setNamedTarget("look_left");
my_group.move();
return false;
}
bool CartesianTrajectoryPlannerModule::plan(vigir_planning_msgs::RequestDrakeCartesianTrajectory &request_message, vigir_planning_msgs::ResultDrakeTrajectory &result_message)
{
// stay at q0 for nominal trajectory
bool received_world_transform = false;
VectorXd q0 = VectorXd::Zero(this->getRobotModel()->num_positions);
q0 = messageQs2DrakeQs(q0, request_message.current_state, received_world_transform);
std::cout << "q0 = " << std::endl;
MatrixXd printQ = q0;
printSortedQs(printQ);
if ( request_message.waypoint_times.empty() ) {
request_message.waypoint_times = estimateWaypointTimes(q0, request_message.target_link_names, request_message.waypoints);
if ( request_message.waypoint_times.empty() ) { // request was invalid
return false;
}
}
std::vector<Waypoint*> waypoints = extractOrderedWaypoints(request_message);
int nq = getRobotModel()->num_positions;
int num_steps = waypoints.size();
bool success = true;
// run inverse kinematics
MatrixXd q_sol(nq,0);
MatrixXd qd_sol(nq,0);
MatrixXd qdd_sol(nq,0);
std::vector<double> t_sol;
int last_successful_waypoint = -1;
for ( int i = 1; i < num_steps; i++ ) {
Waypoint *current_start_point = waypoints[i-1];
Waypoint *current_target_point = waypoints[i];
double time_step = (current_target_point->waypoint_time - current_start_point->waypoint_time) / NUM_TIME_STEPS;
// check trajectory every time_step seconds
std::vector<double> t_vec;
double duration = current_target_point->waypoint_time - current_start_point->waypoint_time;
t_vec.push_back(current_start_point->waypoint_time);
t_vec.push_back(current_start_point->waypoint_time + duration/2.0);
t_vec.push_back(current_target_point->waypoint_time);
t_sol.push_back(current_start_point->waypoint_time);
t_sol.push_back(current_start_point->waypoint_time + duration/2.0);
VectorXd qdot_0 = VectorXd::Zero(this->getRobotModel()->num_velocities);
MatrixXd q_seed = q0.replicate(1, t_vec.size());
MatrixXd q_nom = q_seed;
// build list of constraints from message
IKoptions *ik_options = buildIKOptions(duration);
// build constraints
std::vector<RigidBodyConstraint*> constraints = buildIKConstraints(request_message, current_start_point, current_target_point, q0);
// run IK trajectory calculation
MatrixXd q_sol_part(this->getRobotModel()->num_positions,t_vec.size());
MatrixXd qd_sol_part(this->getRobotModel()->num_positions,t_vec.size());
MatrixXd qdd_sol_part(this->getRobotModel()->num_positions,t_vec.size());
int info;
std::vector<std::string> infeasible_constraints;
inverseKinTraj(this->getRobotModel(),t_vec.size(),t_vec.data(),qdot_0,q_seed,q_nom,constraints.size(),constraints.data(),q_sol_part,qd_sol_part,qdd_sol_part,info,infeasible_constraints,*ik_options);
if(info>=10) { // something went wrong
std::string constraint_string = "";
for (auto const& constraint : infeasible_constraints) { constraint_string += (constraint+" | "); }
ROS_WARN("Step %d / %d: SNOPT info is %d, IK mex fails to solve the problem", i+1, num_steps, info);
ROS_INFO("Infeasible constraints: %s", constraint_string.c_str());
if ( i == 1 || request_message.execute_incomplete_cartesian_plans == false) {
success = false;
break;
}
else {
t_sol.erase(t_sol.end()-1, t_sol.end());
q_sol.conservativeResize(nq, q_sol.cols() + 1 );
qd_sol.conservativeResize(nq, qd_sol.cols() + 1 );
qdd_sol.conservativeResize(nq, qdd_sol.cols() + 1 );
q_sol.block(0, q_sol.cols()-1, nq, 1) = q0;
qd_sol.block(0, q_sol.cols()-1, nq, 1) = MatrixXd::Zero(nq, 1);
qdd_sol.block(0, q_sol.cols()-1, nq, 1) = MatrixXd::Zero(nq, 1);
break;
}
}
int old_q_size = q_sol.cols();
q_sol.conservativeResize(nq, q_sol.cols() + q_sol_part.cols()-1 );
qd_sol.conservativeResize(nq, qd_sol.cols() + qd_sol_part.cols()-1 );
qdd_sol.conservativeResize(nq, qdd_sol.cols() + qdd_sol_part.cols()-1 );
q_sol.block(0, old_q_size, nq, q_sol_part.cols()-1) = q_sol_part.block(0, 0, nq, q_sol_part.cols()-1);
qd_sol.block(0, old_q_size, nq, qd_sol_part.cols()-1) = qd_sol_part.block(0, 0, nq, qd_sol_part.cols()-1);
qdd_sol.block(0, old_q_size, nq, qdd_sol_part.cols()-1) = qdd_sol_part.block(0, 0, nq, qdd_sol_part.cols()-1);
// add final element at the very end
if ( i == num_steps-1 ) {
t_sol.push_back(waypoints[ waypoints.size()-1 ]->waypoint_time);
q_sol.conservativeResize(nq, q_sol.cols() + 1 );
qd_sol.conservativeResize(nq, qd_sol.cols() + 1 );
qdd_sol.conservativeResize(nq, qdd_sol.cols() + 1 );
q_sol.block(0, q_sol.cols()-1, nq, 1) = q_sol_part.block(0, q_sol_part.cols()-1, nq, 1);
qd_sol.block(0, q_sol.cols()-1, nq, 1) = qd_sol_part.block(0, qd_sol_part.cols()-1, nq, 1);
qdd_sol.block(0, q_sol.cols()-1, nq, 1) = qdd_sol_part.block(0, qdd_sol_part.cols()-1, nq, 1);
}
q0 = q_sol_part.col( q_sol_part.cols()-1 );
last_successful_waypoint = i;
}
if ( success ) {
// generate spline from result matrices (default sample rate = 4.0Hz)
if ( request_message.trajectory_sample_rate == 0.0 ) {
request_message.trajectory_sample_rate = 4.0;
}
double time_step = 1.0 / request_message.trajectory_sample_rate;
double duration = waypoints[ last_successful_waypoint]->waypoint_time;
int num_steps = (duration / time_step) + 0.5;
Eigen::VectorXd response_t(num_steps+1);
for ( int i = 0; i <= num_steps; i++) {
response_t(i) = i*time_step;
}
Eigen::VectorXd interpolated_t = Map<VectorXd>(t_sol.data(), t_sol.size());
interpolateTrajectory(q_sol, interpolated_t, response_t, q_sol, qd_sol, qdd_sol);
result_message = buildTrajectoryResultMsg(q_sol, qd_sol, qdd_sol, response_t, request_message.free_joint_names, received_world_transform);
}
else {
// return an invalid message
result_message.is_valid = false;
}
return success;
}
