void ChompParameters::initFromNodeHandle()
{
ros::NodeHandle node_handle("~");
node_handle.param("planning_time_limit", planning_time_limit_, 6.0);
node_handle.param("max_iterations", max_iterations_, 50);
node_handle.param("max_iterations_after_collision_free", max_iterations_after_collision_free_, 5);
node_handle.param("smoothness_cost_weight", smoothness_cost_weight_, 0.1);
node_handle.param("obstacle_cost_weight", obstacle_cost_weight_, 1.0);
node_handle.param("learning_rate", learning_rate_, 0.01);
node_handle.param("animate_path", animate_path_, true);
node_handle.param("add_randomness", add_randomness_, false);
node_handle.param("smoothness_cost_velocity", smoothness_cost_velocity_, 0.0);
node_handle.param("smoothness_cost_acceleration", smoothness_cost_acceleration_, 1.0);
node_handle.param("smoothness_cost_jerk", smoothness_cost_jerk_, 0.0);
node_handle.param("hmc_discretization", hmc_discretization_, 0.01);
node_handle.param("hmc_stochasticity", hmc_stochasticity_, 0.01);
node_handle.param("hmc_annealing_factor", hmc_annealing_factor_, 0.99);
node_handle.param("use_hamiltonian_monte_carlo", use_hamiltonian_monte_carlo_, false);
node_handle.param("ridge_factor", ridge_factor_, 0.0);
node_handle.param("use_pseudo_inverse", use_pseudo_inverse_, false);
node_handle.param("pseudo_inverse_ridge_factor", pseudo_inverse_ridge_factor_, 1e-4);
node_handle.param("animate_endeffector", animate_endeffector_, false);
node_handle.param("animate_endeffector_segment", animate_endeffector_segment_, std::string("r_gripper_tool_frame"));
node_handle.param("joint_update_limit", joint_update_limit_, 0.1);
node_handle.param("collision_clearence", min_clearence_, 0.2);
node_handle.param("collision_threshold", collision_threshold_, 0.07);
node_handle.param("random_jump_amount", random_jump_amount_, 1.0);
node_handle.param("use_stochastic_descent", use_stochastic_descent_, true);
filter_mode_ = false;
}
/***************************************************************************************************
*
* Initializes the ROS node.
*
* The program path of the .hdev file that is to be executed for image acquisition is passed in
* the launchfile (halcon_program_path). Similarly, if the script uses any external procedures,
* a path (halcon_ext_proc_path) is also passed. The halcon script is automaticcaly initialized.
*
* A service is created that can be called to return a 'velocity vector': the next step of
* correction that the robot needs to perform to servo to the target. This vector is calculated
* by 1) taking an new image, 2) calculate the image features, 3) calculate the velocity vector
* based on the image features.
*
**************************************************************************************************/
int main(int argc, char **argv)
{
// Initialize this rosnode with an empty name string. This allow the node's name
// to be set in the launchfile so that multiple copies of this node with different
// names can be launched. Although you would probably not run more than 1 servo node,
// it is good practice to initialize the ros node's name in the launch file.
ros::init(argc, argv, "");
sleep(10);
// The nodehandle is initilized with a tilde ("~") The tilde refers to the node handle's
// namespace. This could be any namespace, which is roughly equivalent to a node name.
// However, the tilde refers to the current node's namespace. Why is this useful? In a
// roslaunch file, you can pass parameters to the system. There are "global" parameters
// that exist in the global, or default, namespace. These fall directly inside the <launch>
// tag. However, you can also put parameters under a <node> tag, and these will be in the
// local, or private, namespace of that node.
ros::NodeHandle node_handle("~");
// Parameters that are initialized at start of the ros node.
// TODO: these should be made specific to a function.
program_lock = false;
initialized = false;
servo_initialized = false;
camera_parameters_loaded = false;
// Service for providing the main output from this node: a velocity vector.
// Only one service can be activated at a time due to equal service names.
velocity_vector_output_service = node_handle.advertiseService("request_servo_velocity_vector", getVelocityVector_Point);
// Initialize frame transformation parameters for simulated camera.
tf::TransformBroadcaster tf_broadcaster;
tf::Transform transform;
simulated_camera_position = tf::Vector3(0.0, 0.0, 0.0) ;
simulated_camera_rotation = tf::Quaternion(0, 0, 0) ;
// Get camera parameters from Application Control node required for servo algorithm.
camera_parameters_client = node_handle.serviceClient<image_acquisition::request_camera_parameters>("/image_acquisition_node/request_camera_parameters");
while(!camera_parameters_loaded)
{
request_camera_parameters();
ros::Rate rate(1.0);
rate.sleep();
}
// The ROS node will now enter the following loop.
ros::Rate rate(10.0);
while (node_handle.ok())
{
// Frame transformations need to be updated constantly, otherwise they will fade out of existence.
transform.setOrigin(simulated_camera_position);
transform.setRotation(simulated_camera_rotation);
tf_broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base", "simulated_camera_frame"));
// Do one spin of the rosnode, e.g. checking and executing callbacks.
// (http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning)
ros::spinOnce();
rate.sleep();
}
return 0;
}
void StompParameters::initFromNodeHandle()
{
ros::NodeHandle node_handle("~");
node_handle.param("planning_time_limit", planning_time_limit_, 1.0);
node_handle.param("max_iterations", max_iterations_, 500);
node_handle.param("max_iterations_after_collision_free", max_iterations_after_collision_free_, 100);
node_handle.param("smoothness_cost_weight", smoothness_cost_weight_, 0.1);
node_handle.param("obstacle_cost_weight", obstacle_cost_weight_, 1.0);
node_handle.param("constraint_cost_weight", constraint_cost_weight_, 1.0);
node_handle.param("torque_cost_weight", torque_cost_weight_, 0.0);
node_handle.param("state_validity_cost_weight", state_validity_cost_weight_, 20.0);
node_handle.param("ignore_state_validity_percent", ignore_state_validity_percent_, 5.0);
node_handle.param("endeffector_velocity_cost_weight", endeffector_velocity_cost_weight_, 1.0);
node_handle.param("learning_rate", learning_rate_, 0.01);
node_handle.param("animate_path", animate_path_, false);
node_handle.param("add_randomness", add_randomness_, true);
node_handle.param("smoothness_cost_velocity", smoothness_cost_velocity_, 0.0);
node_handle.param("smoothness_cost_acceleration", smoothness_cost_acceleration_, 1.0);
node_handle.param("smoothness_cost_jerk", smoothness_cost_jerk_, 0.0);
node_handle.param("hmc_discretization", hmc_discretization_, 0.01);
node_handle.param("hmc_stochasticity", hmc_stochasticity_, 0.01);
node_handle.param("hmc_annealing_factor", hmc_annealing_factor_, 0.99);
node_handle.param("use_hamiltonian_monte_carlo", use_hamiltonian_monte_carlo_, false);
node_handle.param("ridge_factor", ridge_factor_, 0.0);
node_handle.param("use_pseudo_inverse", use_pseudo_inverse_, false);
node_handle.param("pseudo_inverse_ridge_factor", pseudo_inverse_ridge_factor_, 1e-4);
node_handle.param("animate_endeffector", animate_endeffector_, false);
node_handle.param("animate_endeffector_segment", animate_endeffector_segment_, std::string("r_gripper_tool_frame"));
node_handle.param("use_chomp", use_chomp_, false);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lbmpc");
ros::NodeHandle node_handle("mpc");
mpc::model::Model *model = new mpc::example_models::TanksSystem();
mpc::model::Simulator *simulator = new mpc::example_models::TanksSystemSimulator();
mpc::optimizer::Optimizer *optimizer = new mpc::optimizer::qpOASES(node_handle);
mpc::ModelPredictiveControl *mpc = new mpc::LBMPC(node_handle);
mpc->resetMPC(model, optimizer, simulator);
mpc->initMPC();
double x_ref[2] = {7, 7};
double x_meas[2] = {8, 12};
timespec start_rt, end_rt;
clock_gettime(CLOCK_REALTIME, &start_rt);
mpc->updateMPC(x_meas, x_ref);
clock_gettime(CLOCK_REALTIME, &end_rt);
double duration = (end_rt.tv_sec - start_rt.tv_sec) + 1e-9*(end_rt.tv_nsec - start_rt.tv_nsec);
ROS_INFO("The duration of computation of optimization problem is %f seg", duration);
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "");
ros::NodeHandle node_handle("~");
dec_audio::AudioProcessor audio_processor(node_handle);
ROS_VERIFY(audio_processor.initialize());
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "CrossValidator");
ros::NodeHandle node_handle("~");
task_event_detector::init();
task_event_detector::CrossValidator cross_validator(node_handle);
cross_validator.run();
task_event_detector::exit();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "move_group_tutorial");
ros::NodeHandle node_handle("~");
ros::AsyncSpinner spinner(1);
spinner.start();
sleep(20.0);
moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
geometry_msgs::PoseStamped target_pose1;
target_pose1.header.frame_id = "odom";
target_pose1.pose.position.x = 0.128518;
target_pose1.pose.position.y = -0.132719;
target_pose1.pose.position.z = 0.321876;
target_pose1.pose.orientation.w = 0.0125898;
target_pose1.pose.orientation.x = 0.184773;
target_pose1.pose.orientation.y = -0.980428;
target_pose1.pose.orientation.z = -0.0667877;
group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
sleep(5.0);
if (1)
{
ROS_INFO("Visualizing plan 1 (again)");
display_trajectory.trajectory_start = my_plan.start_state_;
display_trajectory.trajectory.push_back(my_plan.trajectory_);
display_publisher.publish(display_trajectory);
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);
}
ros::shutdown();
return 0;
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "TestTaskAccumulator");
ros::NodeHandle node_handle("~");
TaskAccumulator task_accumulator(node_handle);
ros::Duration(1.0).sleep();
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "TaskEventDetector");
ros::NodeHandle node_handle("~");
task_event_detector::init();
task_event_detector::TaskEventDetector task_event_detector(node_handle);
ros::Duration(1.0).sleep();
ros::MultiThreadedSpinner mts;
mts.spin();
task_event_detector::exit();
return 0;
}
/** Fawkes application.
* @param argc argument count
* @param argv array of arguments
*/
int
main(int argc, char **argv)
{
ros::init(argc, argv, "rosfawkes");
fawkes::runtime::InitOptions init_options(argc, argv);
std::string plugins;
if (ros::param::get("~plugins", plugins) && plugins != "") {
if (init_options.has_load_plugin_list()) {
plugins += std::string(",") + init_options.load_plugin_list();
}
init_options.load_plugins(plugins.c_str());
}
try {
int rv = 0;
if (! fawkes::runtime::init(init_options, rv)) {
return rv;
}
} catch (fawkes::Exception &e) {
printf("Fawkes initialization failed, exception follows.\n");
e.print_trace();
return 1;
}
fawkes::LockPtr<ros::NodeHandle> node_handle(new ros::NodeHandle());
fawkes::ROSAspectIniFin ros_aspect_inifin;
ros_aspect_inifin.set_rosnode(node_handle);
fawkes::runtime::aspect_manager->register_inifin(&ros_aspect_inifin);
if (init_options.has_load_plugin_list()) {
fawkes::runtime::logger->log_info("ROS-Fawkes", "Loading plugins: %s",
init_options.load_plugin_list());
}
fawkes::runtime::main_thread->full_start();
fawkes::runtime::logger->log_info("ROS-Fawkes", "ROS-Fawkes %s startup complete",
FAWKES_VERSION_STRING);
ros::spin();
fawkes::runtime::main_thread->cancel();
fawkes::runtime::main_thread->join();
fawkes::runtime::cleanup();
return 0;
}
DockingStationDetector(int argc, char ** argv)
{
ros::init(argc, argv, "dsd");
std::string laserTN;
ros::NodeHandle node_handle("~");
node_handle.getParam("laser", laserTN);
std::cout << laserTN << std::endl;
sub_ = node_handle.subscribe(laserTN, 1, &DockingStationDetector::laserScan_callback, this);
marker_pub_ = node_handle.advertise<visualization_msgs::Marker>("dockingStationMarker", 1);
object_found_threshold_ = 10.f;
nav_to_goal_service_ = node_handle.advertiseService("nav_to_docking_station", &DockingStationDetector::nav_to_ds, this);
dsf_ = new DockingStationFinderSegmented();
ros::spin();
}
int main(int argc,
char **argv)
{
if (argc < 2)
{
printf("ERROR: No node name provided. %i\n", argc);
for (int i = 0; i < argc; i++)
{
printf("argv[%i] = %s\n", i, argv[i]);
}
return -1;
}
printf("Initializing node named: >%s<.\n", argv[1]);
ros::init(argc, argv, argv[1]);
ros::NodeHandle node_handle("~");
ar_target::ARSinglePublisher ar_single(node_handle);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
if(argc < 2)
{
printf("ERROR: No node name provided. %i\n", argc);
for(int i=0; i<argc; i++)
{
printf("argv[%i] = %s\n", i, argv[i]);
}
return -1;
}
printf("Initializing node named: >%s<.\n", argv[1]);
ros::init(argc, argv, argv[1]);
ros::NodeHandle node_handle("~");
// task_event_detector::init();
// task_event_detector::ModelSelectionGridSearchKernel cross_validator(node_handle);
// task_event_detector::exit();
printf("Exiting node named: >%s<.\n", argv[1]);
return 0;
}
int main(int argc,
char ** argv)
{
ros::init(argc, argv, "opt_calibration");
ros::NodeHandle node_handle("~");
try
{
open_ptrack::opt_calibration::OPTCalibrationNode calib_node(node_handle);
if (not calib_node.initialize())
return 1;
calib_node.spin();
}
catch (std::runtime_error & error)
{
ROS_FATAL("Calibration error: %s", error.what());
return 2;
}
return 0;
}
int main(int argc, char **argv)
{
int motion = 0;
std::string initialpath = "";
if (argc >= 2)
{
motion = atoi(argv[1]);
if(argc >=3)
{
initialpath = argv[2];
}
}
printf("%d Motion %d\n", argc, motion);
ros::init(argc, argv, "move_itomp");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("~");
robot_model_loader::RobotModelLoader robot_model_loader(
"robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
planning_scene::PlanningScenePtr planning_scene(
new planning_scene::PlanningScene(robot_model));
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(
new pluginlib::ClassLoader<planning_interface::PlannerManager>(
"moveit_core", "planning_interface::PlannerManager"));
} catch (pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM(
"Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(
planner_plugin_loader->createUnmanagedInstance(
planner_plugin_name));
if (!planner_instance->initialize(robot_model,
node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM(
"Using planning interface '" << planner_instance->getDescription() << "'");
} catch (pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes =
planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0; i < classes.size(); ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM(
"Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
}
loadStaticScene(node_handle, planning_scene, robot_model);
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(1.0);
sleep_time.sleep();
renderStaticScene(node_handle, planning_scene, robot_model);
// We will now create a motion plan request
// specifying the desired pose of the end-effector as input.
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
std::vector<robot_state::RobotState> robot_states;
int state_index = 0;
robot_states.push_back(planning_scene->getCurrentStateNonConst());
robot_states.push_back(robot_states.back());
Eigen::VectorXd start_trans, goal_trans;
// set trajectory constraints
std::vector<Eigen::VectorXd> waypoints;
std::vector<std::string> hierarchy;
// internal waypoints between start and goal
if(initialpath.empty())
{
hierarchy.push_back("base_prismatic_joint_x");
hierarchy.push_back("base_prismatic_joint_y");
hierarchy.push_back("base_prismatic_joint_z");
hierarchy.push_back("base_revolute_joint_z");
hierarchy.push_back("base_revolute_joint_y");
hierarchy.push_back("base_revolute_joint_x");
Eigen::VectorXd vec1;
start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0;
goal_trans = Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0;
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0;
waypoints.push_back(vec1);
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0;
waypoints.push_back(vec1);
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0;
waypoints.push_back(vec1);
}
else
{
hierarchy = InitTrajectoryFromFile(waypoints, initialpath);
start_trans = waypoints.front();
goal_trans = waypoints.back();
}
setWalkingStates(robot_states[state_index], robot_states[state_index + 1],
start_trans, goal_trans, hierarchy);
for (int i = 0; i < waypoints.size(); ++i)
{
moveit_msgs::Constraints configuration_constraint =
setRootJointConstraint(hierarchy, waypoints[i]);
req.trajectory_constraints.constraints.push_back(
configuration_constraint);
}
displayStates(robot_states[state_index], robot_states[state_index + 1],
node_handle, robot_model);
doPlan("whole_body", req, res, robot_states[state_index],
robot_states[state_index + 1], planning_scene, planner_instance);
visualizeResult(res, node_handle, 0, 1.0);
ROS_INFO("Done");
planner_instance.reset();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "move_group_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("motion_planner_api");
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
if (!node_handle.getParam("/move_group/planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch(pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
}
ros::WallDuration sleep_time(15.0);
sleep_time.sleep();
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "odom";
pose.pose.position.x = -0.0119214;
pose.pose.position.y = 0.0972478;
pose.pose.position.z = 0.636616;
pose.pose.orientation.x = 0.273055;
pose.pose.orientation.y = -0.891262;
pose.pose.orientation.z = -0.346185;
pose.pose.orientation.w = 0.106058;
group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
if (1)
{
ROS_INFO("Visualizing plan 1 (again)");
display_trajectory.trajectory_start = my_plan.start_state_;
display_trajectory.trajectory.push_back(my_plan.trajectory_);
display_publisher.publish(display_trajectory);
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);
}
std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
group_variable_values[4] = -1.0;
group.setJointValueTarget(group_variable_values);
success = group.plan(my_plan);
ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");
sleep(5.0);
return 0;
}
bool DMPDualArmIkController::initXml(pr2_mechanism_model::RobotState* robot,
TiXmlElement* config)
{
ros::NodeHandle node_handle(config->Attribute("name"));
return init(robot, node_handle);
}
bool OmplRosIKSampleableRegion::initialize(const ompl::base::StateSpacePtr &state_space,
const std::string &kinematics_solver_name,
const std::string &group_name,
const std::string &end_effector_name,
const planning_environment::CollisionModelsInterface* cmi)
{
collision_models_interface_ = cmi;
state_space_ = state_space;
group_name_ = group_name;
end_effector_name_ = end_effector_name;
ros::NodeHandle node_handle("~");
if(!kinematics_loader_.isClassAvailable(kinematics_solver_name))
{
ROS_ERROR("pluginlib does not have the class %s",kinematics_solver_name.c_str());
return false;
}
try
{
kinematics_solver_ = kinematics_loader_.createClassInstance(kinematics_solver_name);
return false;
}
catch(pluginlib::PluginlibException& ex) //handle the class failing to load
{
ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
return false;
}
std::string base_name, tip_name;
if(!node_handle.hasParam(group_name_+"/root_name"))
{
ROS_ERROR_STREAM("Kinematics solver has no root name " << base_name << " in param ns " << node_handle.getNamespace());
throw new OMPLROSException();
}
node_handle.getParam(group_name_+"/root_name",base_name);
if(!node_handle.hasParam(group_name_+"/tip_name"))
{
ROS_ERROR_STREAM("Kinematics solver has no root name " << tip_name << " in param ns " << node_handle.getNamespace());
throw new OMPLROSException();
}
node_handle.getParam(group_name_+"/tip_name",tip_name);
if(!kinematics_solver_->initialize(group_name,
base_name,
tip_name,
.01))
{
ROS_ERROR("Could not initialize kinematics solver for group %s",group_name.c_str());
return false;
}
// scoped_state_.reset(new ompl::base::ScopedState<ompl::base::CompoundStateSpace>(state_space_));
seed_state_.joint_state.name = kinematics_solver_->getJointNames();
seed_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
solution_state_.joint_state.name = kinematics_solver_->getJointNames();
solution_state_.joint_state.position.resize(kinematics_solver_->getJointNames().size());
if(!ompl_ros_interface::getRobotStateToOmplStateMapping(seed_state_,scoped_state_,robot_state_to_ompl_state_mapping_))
return false;
if(!ompl_ros_interface::getOmplStateToRobotStateMapping(scoped_state_,seed_state_,ompl_state_to_robot_state_mapping_))
return false;
}
int main(int argc, char *argv[])
{
//Initial position
pose.position.x = 0;
pose.position.y = 0;
//Real Position
//pose.position.z = 375;
//Simulation position
//pose.position.z = 804;
//Simulation position with right orientation, we start lower so he cant reach that pos
pose.position.z = 604;
old_pose=pose;
CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle
//Creating the joint_msg_leap
joint_msg_leap.name.resize(8);
joint_msg_leap.position.resize(8);
joint_msg_leap.name[0]="arm_1_joint";
joint_msg_leap.name[1]="arm_2_joint";
joint_msg_leap.name[2]="arm_3_joint";
joint_msg_leap.name[3]="arm_4_joint";
joint_msg_leap.name[4]="arm_5_joint";
joint_msg_leap.name[5]="arm_6_joint";
joint_msg_leap.name[6]="base_joint_gripper_left";
joint_msg_leap.name[7]="base_joint_gripper_right";
aux_enter=1;
FIRST_VALUE=true;//Help knowing Initial Position of the hand
int arm_trajectory_point=1;
/*Finish Variables Initialitation*/
//ROS DECLARATION
ros::init(argc, argv,"listener");
if (argc != 2)
{
ROS_WARN("WARNING: you should specify number of points to capture");
}
else
{
count=atoi(argv[1]);
ROS_INFO ("Number of points /n%d", count);
}
//Create an object of the LeapMotionListener Class
LeapMotionListener leapmotionlistener;
leapmotionlistener.Configure(count);
ros::NodeHandle node_handle("~");
robo_pub = node_handle.advertise<sensor_msgs::JointState>("/joint_leap", 1);
robo_sub = node_handle.subscribe("/joint_states", 1, joinstateCallback);
ros::ServiceClient clientinit = node_handle.serviceClient<sensor_listener::InitHaltAPI>("/InitHaltAPI");
sensor_listener::InitHaltAPI srvinit;
// start a ROS spinning thread
//ros::AsyncSpinner spinner(1);
//we need this for leap
ros::Rate r(10);
ROS_INFO("PRESH LEFT PEDAL TO START");
while((s=getchar())!= '1')
ROS_INFO("PRESH LEFT PEDAL TO START");
//Enable this part to communicate with the real robot
srvinit.request.command=command;
//if (clientinit.call(srvinit))
//{
//ROS_INFO("Init correct");
/* SENSOR SUBSCRIBING */
//LEAP MOTION
ROS_INFO("SUBSCRIBING LEAPMOTION");
ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
ros::ServiceClient client = node_handle.serviceClient<sensor_listener::PositionAPICoordSpaceQuat>("/PositionAPICoordSpaceQuat");
pointerto_client=&client;
leapmotionlistener.setPointertoClient(pointerto_client);
while(!CAPTURE_MOVEMENT==true)
{
ros::spinOnce();
}
leapsub.shutdown();
ROS_INFO("CAPTURING POINTS FINISH");
// End of Capturing Stage
return 0;
/*else
{
ROS_INFO("Could not init");
}*/
}
int main(int argc, char **argv)
{
/*Initialise Variables*/
CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle
//Creating the joint_msg_leap
joint_msg_leap.name.resize(8);
joint_msg_leap.position.resize(8);
joint_msg_leap.name[0]="arm_1_joint";
joint_msg_leap.name[1]="arm_2_joint";
joint_msg_leap.name[2]="arm_3_joint";
joint_msg_leap.name[3]="arm_4_joint";
joint_msg_leap.name[4]="arm_5_joint";
joint_msg_leap.name[5]="arm_6_joint";
joint_msg_leap.name[6]="base_joint_gripper_left";
joint_msg_leap.name[7]="base_joint_gripper_right";
aux_enter=1;
FIRST_VALUE=true;//Help knowing Initial Position of the hand
int arm_trajectory_point=1;
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
/*Finish Variables Initialitation*/
//ROS DECLARATION
ros::init(argc, argv,"listener");
if (argc != 2)
{
ROS_WARN("WARNING: you should specify number of points to capture");
}
else
{
count=atoi(argv[1]);
ROS_INFO ("Number of points /n%d", count);
}
//Create an object of the LeapMotionListener Class
LeapMotionListener leapmotionlistener;
leapmotionlistener.Configure(count);
//ros::NodeHandle node_handle;
ros::NodeHandle node_handle("~");
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
//we need this for leap
ros::Rate r(1);
//robo_pub = n.advertise<sensor_msgs::JointState>("joint_leap", 100);
//Creating a Robot Model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
//Initialise PlanningSceneMonitorPtr
/* MOVEIT Setup*/
// ^^^^^
moveit::planning_interface::MoveGroup group("arm");
group.setPlanningTime(0.5);
moveit::planning_interface::MoveGroup::Plan my_plan;
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to deal directly with the world.
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
/* Adding/Removing Objects and Attaching/Detaching Objects*/
ROS_INFO("CREATING PLANNING_SCENE PUBLISHER");
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
ROS_INFO("CREATING COLLISION OBJECT");
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "gripper_left";
/* The id of the object */
attached_object.object.id = "box";
/* A default pose */
geometry_msgs::Pose posebox;
posebox.orientation.w = 1.0;
/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(posebox);
attached_object.object.operation = attached_object.object.ADD;
ROS_INFO("ADDING COLLISION OBJECT TO THE WORLD");
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(attached_object.object);
planning_scene_msg.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();
/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;
/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene_msg.world.collision_objects.clear();
planning_scene_msg.world.collision_objects.push_back(remove_object);
planning_scene_msg.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();
// Create a publisher for visualizing plans in Rviz.
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model,collision_detection::WorldPtr(new collision_detection::World())));
//Creating planning_scene_monitor
boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(2.0)));
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(planning_scene,"robot_description", tf));
planning_scene_monitor->startSceneMonitor();
//planning_scene_monitor->initialize(planning_scene);
planning_pipeline::PlanningPipeline *planning_pipeline= new planning_pipeline::PlanningPipeline(robot_model,node_handle,"planning_plugin", "request_adapters");
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(20.0);
sleep_time.sleep();
/*end of MOVEIT Setup*/
// We can print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
// Planning to a Pose goal 1
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector
ROS_INFO("Planning to INITIAL POSE");
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "/odom_combined";
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1.15;
pose.pose.orientation.w = 1.0;
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
old_pose=pose;
// We will create the request as a constraint using a helper function available
req.group_name = "arm";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
// Now, call the pipeline and check whether planning was successful.
planning_pipeline->generatePlan(planning_scene, req, res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
// Visualize the result
// ^^^^^^^^^^^^^^^^^^^^
/* Visualize the trajectory */
moveit_msgs::DisplayTrajectory display_trajectory;
ROS_INFO("Visualizing the trajectory 1");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
//sleep_time.sleep();
/* End Planning to a Pose goal 1*/
// First, set the state in the planning scene to the final state of the last plan
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
//planning_scene->setCurrentState(response.trajectory_start);
joint_model_group = robot_state.getJointModelGroup("arm");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
spinner.stop();
/*Capturing Stage*/
/*****************/
ROS_INFO("PRESH ENTER TO START CAPTURING POINTS");
while (getline(std::cin,s))
{
if ('\n' == getchar())
break;
}
/* SENSOR SUBSCRIBING */
//LEAP MOTION
ROS_INFO("SUBSCRIBING LEAPMOTION");
ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
ros::Subscriber trajectorysub = node_handle.subscribe("/move_group/", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
while(!CAPTURE_MOVEMENT==true)
{
ros::spinOnce();
}
leapsub.shutdown();
ROS_INFO("CAPTURING POINTS FINISH...PROCESSING POINTS");
// End of Capturing Stage
/* Start Creating Arm Trajectory*/
/**********************************/
ROS_INFO("START CREATING ARM TRAJECTORY");
for (unsigned i=0; i<trajectory_hand.size(); i++)
{
//First we set the initial Position of the Hand
if (FIRST_VALUE)
{
dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
FIRST_VALUE=0;
ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
sleep(2);
}
else
{
// Both limits for x,y,z to avoid small changes
Updifferencex=dataLastHand_.palmpos.x+10;
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
if ((trajectory_hand.at(i).palmpos.x<Downdifferencex)||(trajectory_hand.at(i).palmpos.x>Updifferencex)||(trajectory_hand.at(i).palmpos.y<Downdifferencey)||(trajectory_hand.at(i).palmpos.y>Updifferencey)||(trajectory_hand.at(i).palmpos.z<Downdifferencez)||(trajectory_hand.at(i).palmpos.z>Updifferencez))
{
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO("TRYING TO ADD POINT %d TO TRAJECTORY",arm_trajectory_point);
// Cartesian Paths
// ^^^^^^^^^^^^^^^
// You can plan a cartesian path directly by specifying a list of waypoints
// for the end-effector to go through. Note that we are starting
// from the new start state above. The initial pose (start state) does not
// need to be added to the waypoint list.
pose.header.frame_id = "/odom_combined";
pose.pose.orientation.w = 1.0;
pose.pose.position.y +=(trajectory_hand.at(i).palmpos.x-dataLastHand_.palmpos.x)/500 ;
pose.pose.position.z +=(trajectory_hand.at(i).palmpos.y-dataLastHand_.palmpos.y)/1000 ;
if(pose.pose.position.z>Uplimitez)
pose.pose.position.z=Uplimitez;
pose.pose.position.x +=-(trajectory_hand.at(i).palmpos.z-dataLastHand_.palmpos.z)/500 ;
ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
ROS_INFO("Palmpos \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
// We will create the request as a constraint using a helper function available
ROS_INFO("1");
pose_goal= kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
ROS_INFO("2");
req.goal_constraints.push_back(pose_goal);
// Now, call the pipeline and check whether planning was successful.
planning_pipeline->generatePlan(planning_scene, req, res);
ROS_INFO("3");
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
pose=old_pose;
}
else
{
// Now when we plan a trajectory it will avoid the obstacle
res.getMessage(response);
collision_result.clear();
collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
robot_state::RobotState copied_state = planning_scene->getCurrentState();
planning_scene->checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Collision Test "<< (collision_result.collision ? "in" : "not in")<< " collision");
arm_trajectory_point++;
// Visualize the trajectory
ROS_INFO("VISUALIZING NEW POINT");
//req.goal_constraints.clear();
display_trajectory.trajectory_start = response.trajectory_start;
ROS_INFO("AXIS 1 NEXT TRAJECTORY IS %f\n",response.trajectory_start.joint_state.position[1] );
display_trajectory.trajectory.push_back(response.trajectory);
// Now you should see two planned trajectories in series
display_publisher.publish(display_trajectory);
planning_scene->setCurrentState(response.trajectory_start);
if (planning_scene_monitor->updatesScene(planning_scene))
{
ROS_INFO("CHANGING STATE");
planning_scene_monitor->updateSceneWithCurrentState();
}
else
{
ROS_ERROR("NOT POSSIBLE TO CHANGE THE SCENE");
}
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
req.goal_constraints.clear();
old_pose=pose;
dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
}
//sleep(2);
spinner.stop();
}
}
}
//ros::Subscriber myogestsub = n.subscribe("/myo_gest", 1000, myogestCallback);
return 0;
}
TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type ) :
initialized(false),
eps(_eps),
maxtime(_maxtime),
solvetype(_type),
work(io_service)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml,full_urdf_xml;
node_handle.param("urdf_xml",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("trac_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string))
{
ROS_FATAL_NAMED("trac_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml,xml_string,std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("trac_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree))
ROS_FATAL("Failed to extract kdl tree from xml robot description");
if(!tree.getChain(base_link, tip_link, chain))
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
std::vector<KDL::Segment> chain_segs = chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
std::vector<double> l_bounds, u_bounds;
lb.resize(chain.getNrOfJoints());
ub.resize(chain.getNrOfJoints());
uint joint_num=0;
for(unsigned int i = 0; i < chain_segs.size(); ++i) {
joint = robot_model.getJoint(chain_segs[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
joint_num++;
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
hasLimits = 1;
}
else {
hasLimits = 0;
}
if(hasLimits) {
lb(joint_num-1)=lower;
ub(joint_num-1)=upper;
}
else {
lb(joint_num-1)=std::numeric_limits<float>::lowest();
ub(joint_num-1)=std::numeric_limits<float>::max();
}
ROS_INFO_STREAM("IK Using joint "<<joint->name<<" "<<lb(joint_num-1)<<" "<<ub(joint_num-1));
}
}
initialize();
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "move_group_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("move_group");
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using a planner is pretty easy. Planners are
// setup as plugins in MoveIt! and you can use the ROS pluginlib
// interface to load any planner that you want to use. Before we
// can load the planner, we need two objects, a RobotModel
// and a PlanningScene.
// We will start by instantiating a
// `RobotModelLoader`_
// object, which will look up
// the robot description on the ROS parameter server and construct a
// :moveit_core:`RobotModel` for us to use.
//
// .. _RobotModelLoader: http://docs.ros.org/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
// Using the :moveit_core:`RobotModel`, we can construct a
// :planning_scene:`PlanningScene` that maintains the state of
// the world (including the robot).
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
// We will now construct a loader to load a planner, by name.
// Note that we are using the ROS pluginlib library here.
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
// We will get the name of planning plugin we want to load
// from the ROS param server, and then load the planner
// making sure to catch all exceptions.
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch(pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
<< "Available plugins: " << ss.str());
}
/* Sleep a little to allow time to startup rviz, etc. */
// ros::WallDuration sleep_time(15.0);
ros::WallDuration sleep_time(1);
sleep_time.sleep();
// Pose Goal
// ^^^^^^^^^
// We will now create a motion plan request for the right arm of the PR2
// specifying the desired pose of the end-effector as input.
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.3;
pose.pose.orientation.w = 1.0;
// A tolerance of 0.01 m is specified in position
// and 0.01 radians in orientation
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
// We will create the request as a constraint using a helper function available
// from the
// `kinematic_constraints`_
// package.
//
// .. _kinematic_constraints: http://docs.ros.org/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
req.group_name = "manipulator";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
// We now construct a planning context that encapsulate the scene,
// the request and the response. We call the planner using this
// planning context
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully: %d", (int) res.error_code_.val);
return 0;
}
// Visualize the result
// ^^^^^^^^^^^^^^^^^^^^
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
sleep_time.sleep();
// Joint Space Goals
// ^^^^^^^^^^^^^^^^^
/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
#if 0
const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("manipulator");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
// Now, setup a joint space goal
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values(7, 0.0);
joint_values[0] = -2.0;
joint_values[3] = -0.2;
joint_values[5] = -0.15;
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
// Call the planner and visualize the trajectory
/* Re-construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
/* Now you should see two planned trajectories in series*/
display_publisher.publish(display_trajectory);
/* We will add more goals. But first, set the state in the planning
scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* Now, we go back to the first goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
// Adding Path Constraints
// ^^^^^^^^^^^^^^^^^^^^^^^
// Let's add a new pose goal again. This time we will also add a path constraint to the motion.
/* Let's create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;
moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
/* First, set the state in the planning scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);
/* But, let's impose a path constraint on the motion.
Here, we are asking for the end-effector to stay level*/
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "torso_lift_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("tool0", quaternion);
// Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
// (the workspace of the robot)
// because of this, we need to specify a bound for the allowed planning volume as well;
// Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
// but that is not being used in this example).
// We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
// when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;
// Call the planner and visualize all the plans created so far.
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
// Now you should see four planned trajectories in series
display_publisher.publish(display_trajectory);
#endif
//END_TUTORIAL
sleep_time.sleep();
ROS_INFO("Done");
planner_instance.reset();
return 0;
}
int main(int argc, char *argv[])
{
node_t *result_hdl;
parse_err_t status;
memory_state_t ms;
tok_state_t ts;
stream_t *stream;
bufstream_t bs;
#if defined(NO_GC_FREELIST)
fmcache_state_t fmc_state;
#endif
if(argc != 2) {
printf("usage: %s 'string'\n", argv[0]);
return -1;
}
dbgtrace_setstream(g_stream_stdout);
dbgtrace_enable( 0
//| TC_MEM_ALLOC
//| TC_GC_TRACING
//| TC_MEM_RC
//| TC_GC_VERBOSE
//| TC_NODE_GC
//| TC_NODE_INIT
//| TC_EVAL
| TC_FMC_ALLOC
);
#if defined(NO_GC_FREELIST)
fmcache_state_init(&fmc_state, libc_malloc_wrap, libc_free_wrap, NULL);
node_memstate_init(&ms, fmcache_request, fmcache_return, &fmc_state);
#else
node_memstate_init(&ms, libc_malloc_wrap, libc_free_wrap, NULL);
#endif
stream = bufstream_init(&bs, argv[1], strlen(argv[1]));
tok_state_init(&ts, stream);
result_hdl = node_handle_new(&ms, NULL);
while((status = parse(&ms, &ts, result_hdl)) != PARSE_END) {
if(status != PARSE_OK) {
printf("parse error line %llu char %llu (offset %llu)\n",
(unsigned long long) tok_state_line(&ts),
(unsigned long long) tok_state_linechr(&ts),
(unsigned long long) tok_state_offset(&ts));
printf("-- %s\n", parse_err_str(status));
return -1;
}
printf("parse result: ");
node_print_pretty_stream(g_stream_stdout,
node_handle(result_hdl), false);
printf("\n");
node_print_recursive_stream(g_stream_stdout, (node_handle(result_hdl)));
node_handle_update(result_hdl, NULL);
}
printf("*** cleanup ***\n");
node_droproot(result_hdl);
memory_gc_cycle(&ms);
memory_gc_cycle(&ms);
printf("*** ending state ***\n");
memory_gc_print_state(&ms, g_stream_stdout);
memory_state_reset(&ms);
#if defined(NO_GC_FREELIST)
fmcache_state_reset(&fmc_state);
#endif
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "motion_planning");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("/move_group");
// ros::NodeHandle node_handle("~");
/* SETUP A PLANNING SCENE*/
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Construct a planning scene - NOTE: this is for illustration purposes only.
The recommended way to construct a planning scene is to use the planning_scene_monitor
to construct it for you.*/
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
/* SETUP THE PLANNER*/
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
/* Get the name of the planner we want to use */
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
/* Make sure to catch all exceptions */
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch(pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
<< "Available plugins: " << ss.str());
}
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(1.0);
sleep_time.sleep();
/* CREATE A MOTION PLAN REQUEST FOR THE RIGHT ARM OF THE PR2 */
/* We will ask the end-effector of the PR2 to go to a desired location */
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
/* A desired pose */
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base_link";
pose.pose.position.x = 0.3;
pose.pose.position.y = -0.3;
pose.pose.position.z = 0.7;
pose.pose.orientation.x = 0.62478;
pose.pose.orientation.y = 0.210184;
pose.pose.orientation.z = -0.7107 ;
pose.pose.orientation.w = 0.245722;
/* A desired tolerance */
std::vector<double> tolerance_pose(3, 0.1);
std::vector<double> tolerance_angle(3, 0.1);
// std::vector<double> tolerance_pose(3, 0.01);
// std::vector<double> tolerance_angle(3, 0.01);
ROS_INFO("marker4");
/*Create the request */
req.group_name = "manipulator";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
ROS_INFO("marker5");
/* Construct the planning context */
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
ROS_INFO("marker6");
/* CALL THE PLANNER */
// context->solve(res);
// ROS_INFO("marker7");
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the generated plan */
/* Publisher for display */
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
sleep_time.sleep();
/* NOW TRY A JOINT SPACE GOAL */
/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
robot_state::JointStateGroup* joint_state_group = robot_state.getJointStateGroup("manipulator");
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, setup a joint space goal*/
robot_state::RobotState goal_state(robot_model);
robot_state::JointStateGroup* goal_group = goal_state.getJointStateGroup("manipulator");
std::vector<double> joint_values(7, 0.0);
// joint_values[0] = 2.0;
joint_values[2] = 1.6;
// joint_values[5] = -0.15;
goal_group->setVariableValues(joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
/* Construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
//Now you should see two planned trajectories in series
display_publisher.publish(display_trajectory);
/* Now, let's try to go back to the first goal*/
/* First, set the state in the planning scene to the final state of the last plan */
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, we go back to the first goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
/* Let's create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;
moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
/* First, set the state in the planning scene to the final state of the last plan */
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);
/* But, let's impose a path constraint on the motion.
Here, we are asking for the end-effector to stay level*/
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "base_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_link", quaternion);
// imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
// (the workspace of the robot)
// because of this, we need to specify a bound for the allowed planning volume as well;
// Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
// but that is not being used in this example).
// We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
// when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = -2.0;
req.workspace_parameters.min_corner.z = 0.2;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
//Now you should see four planned trajectories in series
display_publisher.publish(display_trajectory);
sleep_time.sleep();
ROS_INFO("Done");
planner_instance.reset();
return 0;
}
SNS_IK::SNS_IK(const std::string& base_link, const std::string& tip_link,
const std::string& URDF_param, double loopPeriod, double eps,
sns_ik::VelocitySolveType type) :
m_initialized(false),
m_eps(eps),
m_loopPeriod(loopPeriod),
m_nullspaceGain(1.0),
m_solvetype(type)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml, full_urdf_xml;
node_handle.param("urdf_param",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("sns_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string)) {
ROS_FATAL_NAMED("sns_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml, xml_string, std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("sns_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
ROS_FATAL("Failed to extract kdl tree from xml robot description.");
return;
}
if(!tree.getChain(base_link, tip_link, m_chain)) {
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
}
std::vector<KDL::Segment> chain_segments = m_chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
m_lower_bounds.resize(m_chain.getNrOfJoints());
m_upper_bounds.resize(m_chain.getNrOfJoints());
m_velocity.resize(m_chain.getNrOfJoints());
m_acceleration.resize(m_chain.getNrOfJoints());
m_jointNames.resize(m_chain.getNrOfJoints());
unsigned int joint_num=0;
for(std::size_t i = 0; i < chain_segments.size(); ++i) {
joint = robot_model.getJoint(chain_segments[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
double lower=0; //TODO Better default values? Error if these arent found?
double upper=0;
double velocity=0;
double acceleration=0;
if ( joint->type == urdf::Joint::CONTINUOUS ) {
lower=std::numeric_limits<float>::lowest();
upper=std::numeric_limits<float>::max();
} else {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
velocity = std::fabs(joint->limits->velocity);
}
// Checking the Param server for limit modifications
// and acceleration limits
std::string prefix = urdf_xml + "_planning/joint_limits/" + joint->name + "/";
double ul;
if(node_handle.getParam(prefix + "max_position", ul)){
upper = std::min(upper, ul);
}
double ll;
if(node_handle.getParam(prefix + "min_position", ll)){
lower = std::max(lower, ll);
}
double vel;
if(node_handle.getParam(prefix + "max_velocity", vel)){
if (velocity > 0)
velocity = std::min(velocity, std::fabs(vel));
else
velocity = std::fabs(vel);
}
node_handle.getParam(prefix + "max_acceleration", acceleration);
m_lower_bounds(joint_num)=lower;
m_upper_bounds(joint_num)=upper;
m_velocity(joint_num) = velocity;
m_acceleration(joint_num) = std::fabs(acceleration);
m_jointNames[joint_num] = joint->name;
ROS_INFO("sns_ik: Using joint %s lb: %.3f, ub: %.3f, v: %.3f, a: %.3f", joint->name.c_str(),
m_lower_bounds(joint_num), m_upper_bounds(joint_num), m_velocity(joint_num), m_acceleration(joint_num));
joint_num++;
}
}
initialize();
}
USING_NAMESPACE_QPOASES
int main(int argc, char **argv)
{
ros::init(argc, argv, "mpc");
ros::NodeHandle node_handle("mpc");
boost::scoped_ptr<realtime_tools::RealtimePublisher<mpc::MPCState> > mpc_pub;
mpc_pub.reset(new realtime_tools::RealtimePublisher<mpc::MPCState> (node_handle, "data", 1));
mpc_pub->lock();
mpc_pub->msg_.states.resize(12);
mpc_pub->msg_.reference_states.resize(12);
mpc_pub->msg_.inputs.resize(4);
mpc_pub->unlock();
mpc::model::Model *model_ptr = new mpc::example_models::ArDrone();
mpc::optimizer::Optimizer *optimizer_ptr = new mpc::optimizer::qpOASES(node_handle);
mpc::model::Simulator *simulator_ptr = new mpc::example_models::ArDroneSimulator();
mpc::ModelPredictiveControl *mpc_ptr = new mpc::STDMPC(node_handle);
mpc_ptr->resetMPC(model_ptr, optimizer_ptr, simulator_ptr);
// Operation state
double x_operation[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
// Set the initial conditions as the first linearization points in order to initiate the MPC algorithm
model_ptr->setLinearizationPoints(x_operation);
mpc_ptr->initMPC();
double x_ref[12] = {0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double x_meas[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double *u, *delta_u, *x_bar, *u_bar, *new_state;
double delta_xmeas[12], delta_xref[12];
double sampling_time = 0.0083;
u = new double[4];
new_state = new double[12];
x_bar = new double[12];
u_bar = new double[4];
double t_sim;
timespec start_rt, end_rt;
clock_gettime(CLOCK_REALTIME, &start_rt);
real_t begin, end;
for (int counter = 0; counter < 4000; counter++) {
/*while (ros::ok())*/
t_sim = sampling_time * counter;
if (t_sim < 8){
x_ref[0] = 0.25*t_sim; // referencia de posicion en X
x_ref[1] = 0.; // referencia de posicion en Y
x_ref[3] = 0.25; // referencia de velocidad en X (rampa ascendente)
x_ref[4] = 0.;
}
else if (t_sim >= 8 && t_sim < 16){
x_ref[0] = 2.0; // referencia de posicion en X
x_ref[1] = -0.25*t_sim + 2.0; // referencia de posicion en Y
x_ref[3] = 0.; // referencia de velocidad en X
x_ref[4] = -0.25;
}
else if (t_sim >= 16 && t_sim < 24){
x_ref[0] = -0.25*t_sim + 6.0; // referencia de posicion en X
x_ref[1] = -2.0; // referencia de posicion en Y
x_ref[3] = -0.25; // referencia de velocidad en X
x_ref[4] = 0.;
}
else if (t_sim >= 24 && t_sim < 32){
x_ref[0] = 0.; // referencia de posicion en X
x_ref[1] = 0.25*t_sim - 8.0; // referencia de posicion en Y
x_ref[3] = 0.; // referencia de velocidad en X
x_ref[4] = 0.25;
}
/////////////////////////////////////////////////////////////////////////////////
else {
x_ref[0] = 0.;
x_ref[1] = 0.;
x_ref[3] = 0.;
x_ref[4] = 0.;
}
x_bar = model_ptr->getOperationPointsStates();
for (int i = 0; i < 12; i++) {
delta_xmeas[i] = x_meas[i] - x_bar[i];
delta_xref[i] = x_ref[i] - x_bar[i];
}
// Solving the quadratic problem to obtain the new inputs
begin = getCPUtime();
mpc_ptr->updateMPC(delta_xmeas, delta_xref); // Here we are also recalculating the system matrices A and B
end = getCPUtime();
real_t duration = end - begin;
//std::cout << "Optimization problem computational time:" << static_cast<double>(duration) << std::endl;
delta_u = mpc_ptr->getControlSignal();
u_bar = model_ptr->getOperationPointsInputs();
for (int i = 0; i < 4; i++)
u[i] = u_bar[i] + delta_u[i];
// Updating the simulator with the new inputs
new_state = simulator_ptr->simulatePlant(x_meas, u, sampling_time);
Eigen::Map<Eigen::VectorXd> new_(new_state, 12);
//std::cout << "New state\t" << new_.transpose() << std::endl;
// Setting the new operation points
// model_ptr->setLinearizationPoints(new_state);
if (mpc_pub->trylock()) {
mpc_pub->msg_.header.stamp = ros::Time::now();
for (int j = 0; j < 12; j++) {
mpc_pub->msg_.states[j] = x_meas[j];
mpc_pub->msg_.reference_states[j] = x_ref[j];
}
for (int j = 0; j < 4; j++)
mpc_pub->msg_.inputs[j] = u[j];
}
mpc_pub->unlockAndPublish();
// Shifting the state vector
for (int i = 0; i < 12; i++) {
x_meas[i] = new_state[i];
}
}
//clock_gettime(CLOCK_REALTIME, &end_rt);
//double duration = (end_rt.tv_sec - start_rt.tv_sec) + 1e-9*(end_rt.tv_nsec - start_rt.tv_nsec);
clock_gettime(CLOCK_REALTIME, &end_rt);
double duration = (end_rt.tv_sec - start_rt.tv_sec) + 1e-9*(end_rt.tv_nsec - start_rt.tv_nsec);
ROS_INFO("The duration of computation of optimization problem is %f seg.", duration);
mpc_ptr->writeToDisc();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "workspace_analysis");
ros::AsyncSpinner spinner(1);
spinner.start();
// wait some time for everything to be loaded correctly...
ROS_INFO_STREAM("Waiting a few seconds to load the robot description correctly...");
sleep(3);
ROS_INFO_STREAM("Hope this was enough!");
/*Get some ROS params */
ros::NodeHandle node_handle("~");
double res_x, res_y, res_z;
double min_x, min_y, min_z;
double max_x, max_y, max_z;
double roll, pitch, yaw;
int max_attempts;
double joint_limits_penalty_multiplier;
std::string group_name;
if (!node_handle.getParam("min_x", min_x))
min_x = 0.0;
if (!node_handle.getParam("max_x", max_x))
max_x = 0.0;
if (!node_handle.getParam("res_x", res_x))
res_x = 0.1;
if (!node_handle.getParam("min_y", min_y))
min_y = 0.0;
if (!node_handle.getParam("max_y", max_y))
max_y = 0.0;
if (!node_handle.getParam("res_y", res_y))
res_y = 0.1;
if (!node_handle.getParam("min_z", min_z))
min_z = 0.0;
if (!node_handle.getParam("max_z", max_z))
max_z = 0.0;
if (!node_handle.getParam("res_z", res_z))
res_z = 0.1;
if (!node_handle.getParam("max_attempts", max_attempts))
max_attempts = 10000;
if (!node_handle.getParam("roll", roll))
roll = 0.0;
if (!node_handle.getParam("pitch", pitch))
pitch = 0.0;
if (!node_handle.getParam("yaw", yaw))
yaw = 0.0;
if (!node_handle.getParam("joint_limits_penalty_multiplier", joint_limits_penalty_multiplier))
joint_limits_penalty_multiplier = 0.0;
std::string filename;
if (!node_handle.getParam("filename", filename))
ROS_ERROR("Will not write to file");
std::string quat_filename;
if (!node_handle.getParam("quat_filename", quat_filename))
ROS_ERROR("Will not write to file");
if (!node_handle.getParam("group_name", group_name))
ROS_FATAL("Must have valid group name");
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Create a robot state*/
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
if(!robot_model->hasJointModelGroup(group_name))
ROS_FATAL("Invalid group name: %s", group_name.c_str());
const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name);
/* Construct a planning scene - NOTE: this is for illustration purposes only.
The recommended way to construct a planning scene is to use the planning_scene_monitor
to construct it for you.*/
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
moveit_msgs::WorkspaceParameters workspace;
workspace.min_corner.x = min_x;
workspace.min_corner.y = min_y;
workspace.min_corner.z = min_z;
workspace.max_corner.x = max_x;
workspace.max_corner.y = max_y;
workspace.max_corner.z = max_z;
/* Load the workspace analysis */
moveit_workspace_analysis::WorkspaceAnalysis workspace_analysis(planning_scene, true, joint_limits_penalty_multiplier);
ros::Time init_time;
moveit_workspace_analysis::WorkspaceMetrics metrics;
/* Compute the metrics */
bool use_vigir_rpy = true;
if (use_vigir_rpy) {
std::vector<geometry_msgs::Quaternion> orientations;
geometry_msgs::Quaternion quaternion;
//yaw = -M_PI*0.0;
//roll = M_PI*0.49;
// translate roll, pitch and yaw into a Quaternion
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(q, quaternion);
orientations.push_back(quaternion);
metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);
} else {
// load the set of quaternions
std::vector<geometry_msgs::Quaternion> orientations;
std::ifstream quat_file;
quat_file.open(quat_filename.c_str());
while(!quat_file.eof())
{
geometry_msgs::Quaternion temp_quat;
orientations.push_back(temp_quat);
}
init_time = ros::Time::now();
metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);
if(metrics.points_.empty())
ROS_WARN_STREAM("No point to be written to file: consider changing the workspace, or recompiling moveit_workspace_analysis with a longer sleeping time at the beginning (if this could be the cause)");
}
//ros::WallDuration duration(100.0);
//moveit_workspace_analysis::WorkspaceMetrics metrics = workspace_analysis.computeMetricsFK(&(*robot_state), joint_model_group, max_attempts, duration);
if(!filename.empty())
if(!metrics.writeToFile(filename,",",false))
ROS_INFO("Could not write to file");
ros::Duration total_duration = ros::Time::now() - init_time;
ROS_INFO_STREAM("Total duration: " << total_duration.toSec() << "s");
ros::shutdown();
return 0;
}
