TEST_F(CollisionDetectionDistanceFieldTester, DefaultNotInCollision)
{
robot_state::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
crobot_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_FALSE(res.collision);
}
void MotionPlanningFrame::setAsGoalStateButtonClicked()
{
QListWidgetItem *item = ui_->list_states->currentItem();
if (item)
{
robot_state::RobotState robot_state(*planning_display_->getQueryGoalState());
robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state);
planning_display_->setQueryGoalState(robot_state);
}
}
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(
const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
ConstraintApproximationConstructionResults& result)
{
// state storage structure
ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
ob::StateStoragePtr sstor(cass);
// construct a sampler for the sampling constraints
kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
kset.add(constr_hard, no_transforms);
const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();
unsigned int attempts = 0;
double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
bounds_val);
pcontext->getOMPLStateSpace()->setup();
// construct the constrained states
robot_state::RobotState robot_state(default_state);
const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
ConstrainedSampler* csmp = nullptr;
if (csmng)
{
constraint_samplers::ConstraintSamplerPtr cs =
csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
if (cs)
csmp = new ConstrainedSampler(pcontext.get(), cs);
}
ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
int done = -1;
bool slow_warn = false;
ompl::time::point start = ompl::time::now();
while (sstor->size() < options.samples)
{
++attempts;
int done_now = 100 * sstor->size() / options.samples;
if (done != done_now)
{
done = done_now;
ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done,
100.0 * (double)sstor->size() / (double)attempts);
}
if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
{
slow_warn = true;
ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow...");
}
if (attempts > options.samples && sstor->size() == 0)
{
ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples");
break;
}
ss->sampleUniform(temp.get());
pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get());
if (kset.decide(robot_state).satisfied)
{
if (sstor->size() < options.samples)
{
temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
sstor->addState(temp.get());
}
}
}
result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(),
result.state_sampling_time);
if (csmp)
{
result.sampling_success_rate = csmp->getConstrainedSamplingRate();
ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate);
}
result.milestones = sstor->size();
if (options.edges_per_sample > 0)
{
ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...",
options.edges_per_sample);
// construct connexions
const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
unsigned int milestones = sstor->size();
std::vector<ob::State*> int_states(options.max_explicit_points, nullptr);
pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);
ompl::time::point start = ompl::time::now();
int good = 0;
int done = -1;
for (std::size_t j = 0; j < milestones; ++j)
{
int done_now = 100 * j / milestones;
if (done != done_now)
{
done = done_now;
ROS_INFO_NAMED("constraints_library", "%d%% complete", done);
}
if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
continue;
const ob::State* sj = sstor->getState(j);
for (std::size_t i = j + 1; i < milestones; ++i)
{
if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
continue;
double d = space->distance(sstor->getState(i), sj);
if (d >= options.max_edge_length)
continue;
unsigned int isteps =
std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
double step = 1.0 / (double)isteps;
bool ok = true;
space->interpolate(sstor->getState(i), sj, step, int_states[0]);
for (unsigned int k = 1; k < isteps; ++k)
{
double this_step = step / (1.0 - (k - 1) * step);
space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, int_states[k]);
if (!kset.decide(robot_state).satisfied)
{
ok = false;
break;
}
}
if (ok)
{
cass->getMetadata(i).first.push_back(j);
cass->getMetadata(j).first.push_back(i);
if (options.explicit_motions)
{
cass->getMetadata(i).second[j].first = sstor->size();
for (unsigned int k = 0; k < isteps; ++k)
{
int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
sstor->addState(int_states[k]);
}
cass->getMetadata(i).second[j].second = sstor->size();
cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
}
good++;
if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
break;
}
}
}
result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions",
result.state_connection_time, good);
pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);
return sstor;
}
// TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic
// Update with more intelligent logic as needed
ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here.");
return sstor;
}
int main(int argc, char ** argv)
{
ros::init(argc,argv,"");
int num_joints = 28;
int steps = 5000; // simulate 5 seconds of data
double dt = 0.001;
double elapsed = dt*(steps-1);
std::cout << "Define test of " << steps << " steps at dt=" << dt << "with " << num_joints << " joints" << std::endl;
vigir_control::VigirRobotPoseFilter pose_filter("SimpleFilter");
vigir_control::VigirRobotStateData robot_state(num_joints); // state estimate
/* Choose Normal Distribution and create generator*/
NormalDistribution gaussian_dist_05(0.0, 0.5);
GaussianGenerator generator_05(rng, gaussian_dist_05);
NormalDistribution gaussian_dist_01(0.0, 0.1);
GaussianGenerator generator_01(rng, gaussian_dist_01);
NormalDistribution gaussian_dist_001(0.0, 0.01);
GaussianGenerator generator_001(rng, gaussian_dist_001);
NormalDistribution gaussian_dist_002(0.0, 0.02);
GaussianGenerator generator_002(rng, gaussian_dist_002);
NormalDistribution gaussian_dist_005(0.0, 0.05);
GaussianGenerator generator_005(rng, gaussian_dist_005);
NormalDistribution gaussian_dist_15(0.0, 1.5);
GaussianGenerator generator_15(rng, gaussian_dist_15);
// calculate_test_data(actual, control, 0.0);
// sense_test_data(sensed, actual, generator_05, generator_01, generator_01);
// sense_test_data(control, control, generator_001, generator_002,generator_002); // bad process control
// sense_test_data(estimated, sensed, generator_15 , generator_15, generator_15); // initial with bad data
// Define vectors to store data for plotting
std::cout << "Define vectors to store data" << std::endl;
std::vector<vigir_control::Pose> actual_poses;
std::vector<vigir_control::Twist> actual_twists;
std::vector<vigir_control::Pose> sensed_poses;
std::vector<vigir_control::Twist> sensed_twists;
std::vector<vigir_control::Pose> estimated_poses;
std::vector<vigir_control::Twist> estimated_twists;
// Initialize vector to hold data for plotting results
std::cout << "Initialize vectors" << std::endl;
// actual_positions.resize(steps, actual.joint_positions_);
// actual_velocities.resize(steps, actual.joint_velocities_);
// sensed_positions.resize(steps, sensed.joint_positions_);
// sensed_velocities.resize(steps, sensed.joint_velocities_);
// estimated_positions.resize(steps, estimated.joint_positions_);
// estimated_velocities.resize(steps,estimated.joint_velocities_);
Timing prediction_timing_("BasicKF:: prediction",true,false);
Timing correction_timing_("BasicKF:: correction",true,false);
std::cout << "Begin simulation loop ..." << std::endl;
for (int i = 1; i < steps; ++i)
{
printf("\r Step %d",i);fflush(stdout);
// Calculate based on actual dynamics model
//printf("\n Calc %d",i);fflush(stdout);
//calculate_test_data(actual, control, i*dt);
// Sense the actual data with noise
//printf("\n Sense %d",i);fflush(stdout);
//sense_test_data(sensed, actual, generator_005, generator_01, generator_01);
// Corrupt the control used for prediction
//printf("\n Control %d",i);fflush(stdout);
//sense_test_data(control, control, generator_001, generator_002, generator_002); // bad process control
//printf("\n Predict %d",i);fflush(stdout);
{DO_TIMING(prediction_timing_)
// basic_kf.predict_filter(estimated.joint_positions_,
// estimated.joint_velocities_,
// control.joint_accelerations_,
// dt);
}
///printf("\n Correct %d",i);fflush(stdout);
{DO_TIMING(correction_timing_)
// basic_kf.correct_filter(estimated.joint_positions_,
// estimated.joint_velocities_,
// sensed.joint_positions_,
// sensed.joint_velocities_);
}
//printf("\n Loop %d",i);fflush(stdout);
// Store data for later plotting
// actual_poses[i] = actual.joint_positions_;
// actual_twists[i] = actual.joint_velocities_;
// sensed_poses[i] = sensed.joint_positions_;
// sensed_twists[i] = sensed.joint_velocities_;
// estimated_poses[i] = estimated.joint_positions_;
// estimated_twists[i] = estimated.joint_velocities_;
}
std::cout << "\nWrite out plot files in python ..." << std::endl;
//for (int i = 0; i < num_joints; ++i)
{ // For each joint create a python file to plot the data
std::stringstream ss;
ss << 0;//i;
std::ofstream out;
std::string filename = "joint_"+ss.str()+".py";
out.open(filename.c_str(), std::ofstream::out);
out << "import numpy as np" << std::endl;
out << "import matplotlib.pyplot as plt" << std::endl;
out << "t=np.linspace(0, " << elapsed << ", " << steps<< ")" << std::endl << std::endl;
// Store data in python file
// store_data(out, "q_act", actual_positions, i);
// store_data(out, "dq_act",actual_velocities, i);
// store_data(out, "q_sensed", sensed_positions, i);
// store_data(out, "dq_sensed",sensed_velocities, i);
// store_data(out, "q_estimated", estimated_positions, i);
// store_data(out, "dq_estimated",estimated_velocities, i);
// // plot the data
// out << std::endl;
// out << "plt.subplot(2,1,1)" << std::endl;
// out << "plt.plot(t,q_act,'g-',label=\"q_act\")" << std::endl;
// out << "plt.plot(t,q_sensed,'r:',label=\"q_sensed\")" << std::endl;
// out << "plt.plot(t,q_estimated,'b:',label=\"q_est\")" << std::endl;
// out << "plt.title(\"Estimation of Joint " << ss.str() <<"\")" << std::endl;
// out << "plt.ylabel(\"position\")" << std::endl;
// out << "plt.legend(loc='upper right', shadow=True)" << std::endl;
// out << "plt.subplot(2,1,2)" << std::endl;
// out << "plt.plot(t,dq_act,'g-',label=\"dq_act\")" << std::endl;
// out << "plt.plot(t,dq_sensed,'r:',label=\"dq_sensed\")" << std::endl;
// out << "plt.plot(t,dq_estimated,'b:',label=\"dq_est\")" << std::endl;
// out << "plt.title(\"Estimation of Joint " << ss.str() <<"\")" << std::endl;
// out << "plt.ylabel(\"velocity\")" << std::endl;
// out << "plt.xlabel(\"time (s)\")" << std::endl;
// out << "plt.legend(loc='upper right', shadow=True)" << std::endl;
// out << std::endl;
// out << "plt.show()" << std::endl;
out.close();
}
std::cout << "\nDone!" << std::endl << std::endl << std::endl << std::endl;
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;
}
