///---------------------------------------------------------------------------- aid_t coroutine_stackful_actor::recv(response_t res, message& msg, duration_t tmo) { aid_t sender; recving_res_ = res; if (!mb_.pop(res, msg)) { if (tmo > zero) { detail::scoped_bool<bool> scp(responsing_); if (tmo < infin) { start_recv_timer(tmo); } actor_code ac = yield(); if (ac == actor_timeout) { return sender; } res = recving_res_; msg = recving_msg_; recving_res_ = response_t(); recving_msg_ = message(); } else { return sender; } } sender = res.get_aid(); return sender; }
///---------------------------------------------------------------------------- void coroutine_stackful_actor::on_free() { base_type::on_free(); stat_ = ready; f_.clear(); curr_match_.clear(); recving_ = false; responsing_ = false; recving_rcv_ = detail::recv_t(); recving_res_ = response_t(); recving_msg_ = message(); ec_ = exit_normal; exit_msg_.clear(); }
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; }