int main(int argc, char **argv)
{
ros::init(argc, argv, PLANNER_NODE_NAME);
bool debug = false;
for (int i = 1 ; i < argc ; ++i)
if (strncmp(argv[i], "--debug", 7) == 0)
debug = true;
ros::AsyncSpinner spinner(1);
spinner.start();
boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf);
if (psm.getPlanningScene())
{
psm.startWorldGeometryMonitor();
psm.startSceneMonitor();
psm.startStateMonitor();
OMPLPlannerService pservice(psm, debug);
pservice.status();
ros::waitForShutdown();
}
else
ROS_ERROR("Planning scene not configured");
return 0;
}
int main(int argc,char* args[])
{
SDL_Rect dim={720,480,32};
PHYSIM psm(NULL,dim,32);
SDL_FillRect(psm.scr,&psm.scr->clip_rect,SDL_MapRGB(psm.scr->format,0,0xFF,0xFF));
while(!psm.ended)
{
//=================================initialisation
//=================================
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~physics simulation
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//.................................graphic rendering
//.................................
//---------------------------------termination
//---------------------------------
}
SDL_Delay(1000);
return 0;
}
// Sequential solver.
bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
float32 invMassA = bodyA->m_mass * bodyA->m_invMass;
float32 invIA = bodyA->m_mass * bodyA->m_invI;
float32 invMassB = bodyB->m_mass * bodyB->m_invMass;
float32 invIB = bodyB->m_mass * bodyB->m_invI;
// Solve normal constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2PositionSolverManifold psm(c,j);
//psm.Initialize(c, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - bodyA->m_sweep.c;
b2Vec2 rB = point - bodyB->m_sweep.c;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
bodyA->m_sweep.c -= invMassA * P;
bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
bodyA->SynchronizeTransform();
bodyB->m_sweep.c += invMassB * P;
bodyB->m_sweep.a += invIB * b2Cross(rB, P);
bodyB->SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
inline
#endif
int addok(list_node *head, int i, int j) {
int i2, j2=j-1;
if (head==NULL && j==0) return 1;
else while (head!=0) {
i2 = head->row;
if (i == i2 || j == j2 || i == i2 - (j - j2) || i == i2 + (j - j2))
return 0;
head = head->next;
j2--;
}
#ifdef DEBUG
if (j2!=-1) {
int tmp = 1;
psm(tmp,addok_error);
psm(j2,addok_error_amount);
}
#endif
return 1;
}
void findSelfCollisionAndDisplayContacts()
{
ros::NodeHandle nh;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
ros::Publisher pub_scene = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::Publisher pub_markers = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 5);
sleep(1);
collision_detection::CollisionRequest req;
do
{
collision_detection::CollisionResult res;
scene->getCurrentState().setToRandomValues();
scene->checkSelfCollision(req, res);
if (res.collision)
break;
} while (true);
moveit_msgs::PlanningScene psmsg;
scene->getPlanningSceneMsg(psmsg);
pub_scene.publish(psmsg);
req.contacts = true;
req.max_contacts = 1000;
req.max_contacts_per_pair = 500;
collision_detection::CollisionResult res;
req.verbose = true;
scene->checkSelfCollision(req, res);
std_msgs::ColorRGBA color;
color.r = 1.0f;
color.g = 0.0f;
color.b = 0.0f;
color.a = 1.0f;
visualization_msgs::MarkerArray arr;
collision_detection::getCollisionMarkersFromContacts(arr, "/odom_combined", res.contacts, color, ros::Duration(30.0));
pub_markers.publish(arr);
ros::Duration(10).sleep();
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
planning_scene_monitor::PlanningSceneMonitor psm("robot_description");
psm.startStateMonitor();
sleep(1);
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_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::PlanningScene planning_scene(kinematic_model);
// planning_scene::PlanningScene planning_scene = *psm.getPlanningScene();
/* The planning scene contains a RobotState *representation of the robot configuration
We can get a reference to it.*/
robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
/* COLLISION CHECKING */
/* Let's check if the current state is in self-collision. All self-collision checks use an unpadded version of the
robot collision model, i.e. no extra padding is applied to the robot.*/
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 1 : Current(joint_state_values.front() > 0.0 state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/* Let's change the current state that the planning scene has and check if that is in self-collision*/
current_state.setToRandomValues();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 2 : Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/* Now, we will do collision checking only for the right_arm of the PR2, i.e. we will check whether
there are any collisions between the right arm and other parts of the body of the robot.*/
collision_request.group_name = "manipulator";
current_state.setToRandomValues();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/* We will first manually set the right arm to a position where we know internal (self) collisions
do happen.*/
std::vector<double> joint_values;
robot_state::JointStateGroup* joint_state_group = current_state.getJointStateGroup("manipulator");
joint_state_group->getVariableValues(joint_values);
joint_values[0] = 1.57; //hard-coded since we know collisions will happen here
joint_state_group->setVariableValues(joint_values);
/* Note that this state is now actually outside the joint limits of the PR2, which we can also check for
directly.*/
ROS_INFO_STREAM("Current state is " << (current_state.satisfiesBounds() ? "valid" : "not valid"));
/* Now, we will try and get contact information for any collisions that
might have happened at a given configuration of the right arm. */
collision_request.contacts = true;
collision_request.max_contacts = 1000;
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 4: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
for(collision_detection::CollisionResult::ContactMap::const_iterator it = collision_result.contacts.begin();
it != collision_result.contacts.end();
++it)
{
ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str());
}
/* We will pass in a changed collision matrix to allow the particular set of contacts that just happened*/
collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();
robot_state::RobotState copied_state = planning_scene.getCurrentState();
for(collision_detection::CollisionResult::ContactMap::const_iterator it = collision_result.contacts.begin();
it != collision_result.contacts.end();
++it)
{
acm.setEntry(it->first.first, it->first.second, true);
}
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/* While we have been checking for self-collisions, we can use the checkCollision functions instead which will
check for both self-collisions and for collisions with the environment (which is currently empty).
This is the set of collision checking functions that you will use most often in a planner. Note that collision checks
with the environment will use the padded version of the robot. Padding helps in keeping the robot further away from
int_state_values.front() > 0.0
obstacles in the environment.*/
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/* CONSTRAINT CHECKING*/
/* Let's first create a constraint for the end-effector of the right arm of the PR2 */
const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");
std::string end_effector_name = joint_model_group->getLinkModelNames().back();
geometry_msgs::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.75;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 1.3;
desired_pose.header.frame_id = "base_link";
moveit_msgs::Constraints goal_constraint = kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);
/* Now, we can directly check it for a state using the PlanningScene class*/
copied_state.setToRandomValues();
bool state_constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
ROS_INFO_STREAM("Test 7: Random state is " << (state_constrained ? "constrained" : "not constrained"));
/* There's a more efficient way of checking constraints (when you want to check the same constraint over
and over again, e.g. inside a planner). We first construct a KinematicConstraintSet which pre-processes
the ROS Constraints messages and sets it up for quick processing. */
kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());
bool state_constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);
ROS_INFO_STREAM("Test 8: Random state is " << (state_constrained_2 ? "constrained" : "not constrained"));
/* There's an even more efficient way to do this using the KinematicConstraintSet class directly.*/
kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state);
ROS_INFO_STREAM("Test 9: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained"));
/* Now, lets try a user-defined callback. This is done by specifying the callback using the
setStateFeasibilityPredicate function to which you pass the desired callback.
Now, whenever anyone calls isStateFeasible, the callback will be checked.*/
planning_scene.setStateFeasibilityPredicate(userCallback);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
ROS_INFO_STREAM("Test 10: Random state is " << (state_feasible ? "feasible" : "not feasible"));
/* Whenever anyone calls isStateValid, three checks are conducted: (a) collision checking (b) constraint checking and
(c) feasibility checking using the user-defined callback */
bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "manipulator");
ROS_INFO_STREAM("Test 10: Random state is " << (state_valid ? "valid" : "not valid"));
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "initialize_demo_db", ros::init_options::AnonymousName);
boost::program_options::options_description desc;
desc.add_options()
("help", "Show help message")
("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
boost::program_options::variables_map vm;
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
boost::program_options::notify(vm);
if (vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
if (!psm.getPlanningScene())
{
ROS_ERROR("Unable to initialize PlanningSceneMonitor");
return 1;
}
bool done = false;
unsigned int attempts = 0;
while (!done && attempts < 5)
{
attempts++;
try
{
moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
pss.reset();
cs.reset();
rs.reset();
// add default planning scenes
moveit_msgs::PlanningScene psmsg;
psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
psmsg.name = "default";
pss.addPlanningScene(psmsg);
ROS_INFO("Added default scene: '%s'", psmsg.name.c_str());
moveit_msgs::RobotState rsmsg;
robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg);
rs.addRobotState(rsmsg, "default");
ROS_INFO("Added default state");
const std::vector<std::string> &gnames = psm.getRobotModel()->getJointModelGroupNames();
for (std::size_t i = 0 ; i < gnames.size() ; ++i)
{
const robot_model::JointModelGroup *jmg = psm.getRobotModel()->getJointModelGroup(gnames[i]);
if (!jmg->isChain())
continue;
const std::vector<std::string> &lnames = jmg->getLinkModelNames();
if (lnames.empty())
continue;
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = lnames.back();
ocm.header.frame_id = psm.getRobotModel()->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
ocm.weight = 1.0;
moveit_msgs::Constraints cmsg;
cmsg.orientation_constraints.resize(1, ocm);
cmsg.name = ocm.link_name + ":upright";
cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName());
ROS_INFO("Added default constraint: '%s'", cmsg.name.c_str());
}
done = true;
ROS_INFO("Default MoveIt! Warehouse was reset. Done.");
}
catch(mongo_ros::DbConnectException &ex)
{
ROS_WARN("MongoDB does not appear to be initialized yet. Waiting for a few seconds before trying again ...");
ros::WallDuration(15.0).sleep();
}
}
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
ros::NodeHandle nh;
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
planning_scene_monitor::PlanningSceneMonitor psm("robot_description");
psm.startStateMonitor();
sleep(1);
robot_state::RobotState current_state = psm.getPlanningScene()->getCurrentState();
std::vector<double>joint_state_values;
current_state.getStateValues(joint_state_values);
for(int i = 0; i < joint_state_values.size(); ++i)
{
ROS_INFO("Joint %i: %f", i, joint_state_values[i]);
}
// this connects to a running instance of the move_group node
move_group_interface::MoveGroup group("manipulator");
group.setStartState(current_state);
ROS_INFO_STREAM("reference frame: " << group.getPoseReferenceFrame());
// if using Eigen type: tf:poseEigenToMsg()
std::vector<geometry_msgs::Pose> waypoints;
double eef_step = 0.1;
double jump_threshold = 1000;
moveit_msgs::RobotTrajectory trajectory;
geometry_msgs::Pose pose0, pose1, pose2;
group.setEndEffectorLink("ee_link");
pose0 = group.getCurrentPose("ee_link").pose;
ROS_INFO_STREAM("Starting Pose:" << pose0);
pose1.position.x = -0.12;
pose1.position.y = -0.4;
pose1.position.z = 0.7;
pose2.position.x = 0.12;
pose2.position.y = -0.4;
pose2.position.z = 0.42;
// Face the wall
pose1.orientation.x = 0.05;
pose1.orientation.y = 0.04;
pose1.orientation.z = -0.82;
pose1.orientation.w = 0.388 ;
pose2.orientation.x = 0.05;
pose2.orientation.y = 0.04;
pose2.orientation.z = -0.82;
pose2.orientation.w = 0.388 ;
waypoints.push_back(pose0);
waypoints.push_back(pose1);
waypoints.push_back(pose2);
// group.setStartState(current_state);
// group.setStartStateToCurrentState();
// group.setWorkspace(0,0,0.1,1.0,1.0,1.0);
group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_STREAM("trajectory: " << trajectory);
ros::ServiceClient client = nh.serviceClient<moveit_msgs::ExecuteKnownTrajectory>("/execute_kinematic_path");
moveit_msgs::ExecuteKnownTrajectory::Request request;
moveit_msgs::ExecuteKnownTrajectory::Response response;
request.trajectory = trajectory;
request.wait_for_execution = false;
client.call(request,response);
ROS_INFO_STREAM("Result: " << ((response.error_code.val == response.error_code.SUCCESS) ? "True " : "False ") << response.error_code.val);
ros::waitForShutdown();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "evaluate_collision_checking_speed");
unsigned int nthreads = 2;
unsigned int trials = 10000;
boost::program_options::options_description desc;
desc.add_options()
("nthreads", boost::program_options::value<unsigned int>(&nthreads)->default_value(nthreads), "Number of threads to use")
("trials", boost::program_options::value<unsigned int>(&trials)->default_value(trials), "Number of collision checks to perform with each thread")
("wait", "Wait for a user command (so the planning scene can be updated in thre background)")
("help", "this screen");
boost::program_options::variables_map vm;
boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
boost::program_options::store(po, vm);
boost::program_options::notify(vm);
if (vm.count("help"))
{
std::cout << desc << std::endl;
return 0;
}
ros::AsyncSpinner spinner(1);
spinner.start();
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
if (psm.getPlanningScene())
{
if (vm.count("wait"))
{
psm.startWorldGeometryMonitor();
psm.startSceneMonitor();
std::cout << "Listening to planning scene updates. Press Enter to continue ..." << std::endl;
std::cin.get();
}
else
ros::Duration(0.5).sleep();
std::vector<robot_state::RobotStatePtr> states;
ROS_INFO("Sampling %u valid states...", nthreads);
for (unsigned int i = 0 ; i < nthreads ; ++i)
{
// sample a valid state
robot_state::RobotState *state = new robot_state::RobotState(psm.getPlanningScene()->getRobotModel());
collision_detection::CollisionRequest req;
do
{
state->setToRandomPositions();
collision_detection::CollisionResult res;
psm.getPlanningScene()->checkCollision(req, res);
if (!res.collision)
break;
} while (true);
states.push_back(robot_state::RobotStatePtr(state));
}
std::vector<boost::thread*> threads;
for (unsigned int i = 0 ; i < states.size() ; ++i)
threads.push_back(new boost::thread(boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get())));
for (unsigned int i = 0 ; i < states.size() ; ++i)
{
threads[i]->join();
delete threads[i];
}
}
else
ROS_ERROR("Planning scene not configured");
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
ros::NodeHandle nh;
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
planning_scene_monitor::PlanningSceneMonitor psm("robot_description");
psm.startStateMonitor();
sleep(1);
robot_state::RobotState current_state = psm.getPlanningScene()->getCurrentState();
std::vector<double>joint_state_values;
current_state.getStateValues(joint_state_values);
for(int i = 0; i < joint_state_values.size(); ++i)
{
ROS_INFO("Joint %i: %f", i, joint_state_values[i]);
}
// this connects to a running instance of the move_group node
move_group_interface::MoveGroup group("manipulator");
group.setStartState(current_state);
ROS_INFO_STREAM("pose reference frame: " << group.getPoseReferenceFrame());
ROS_INFO_STREAM("planning reference frame: " << group.getPlanningFrame());
ROS_INFO_STREAM("Joint goal position tolerance: " << group.getGoalPositionTolerance());
ROS_INFO_STREAM("Joint goal orientation tolerance: " << group.getGoalOrientationTolerance());
// tf::TransformListener tf_listener;
// tf::StampedTransform eef_to_world;
// geometry_msgs::Pose eef_pose;
// tf_listener.lookupTransform(group.getPlanningFrame(), group.getEndEffectorLink(), ros::Time(0), eef_to_world);
// tf::poseTFToMsg(eef_to_world, eef_pose);
//
// ROS_INFO_STREAM("tf pose:" << eef_pose);
group.setEndEffectorLink("ee_link");
ROS_INFO_STREAM("Current Pose:" << group.getCurrentPose());
// group.setPositionTarget(0.50, -0.25, 0.50);
std::vector<double> rpy = group.getCurrentRPY();
double target_roll = rpy[0];
double target_pitch = rpy[1];
double target_yaw = rpy[2];
// group.setRPYTarget(target_roll, target_pitch, target_yaw);
geometry_msgs::Pose target_pose;
target_pose.position.x = 0.30;
target_pose.position.y = -0.504;
target_pose.position.z = 0.50;
target_pose.orientation = group.getCurrentPose().pose.orientation;
// target_pose.orientation.x = 0.370392;
// target_pose.orientation.y = -0.306641;
// target_pose.orientation.z = 0.827324;
// target_pose.orientation.w = 0.290373;
group.setPoseTarget(target_pose);
moveit_msgs::Constraints constraint_msg;
/* Set constraint to face the workpiece */
moveit_msgs::OrientationConstraint face_wp_constraint;
face_wp_constraint.link_name = group.getEndEffectorLink();
face_wp_constraint.header.frame_id = group.getPlanningFrame();
face_wp_constraint.orientation = target_pose.orientation;
face_wp_constraint.absolute_x_axis_tolerance = boost::math::constants::pi<double>()*0.5;
face_wp_constraint.absolute_y_axis_tolerance = boost::math::constants::pi<double>()*0.5;
face_wp_constraint.absolute_z_axis_tolerance = boost::math::constants::pi<double>()*0.5;
face_wp_constraint.weight = 1.0;
constraint_msg.orientation_constraints.push_back(face_wp_constraint);
/* Set constraint to keep end eff below wrist1 joint */
moveit_msgs::JointConstraint wrist_constraint;
wrist_constraint.joint_name = "wrist_1_joint";
wrist_constraint.position = joint_state_values[3];
wrist_constraint.tolerance_above = boost::math::constants::pi<double>()*0.5;
wrist_constraint.tolerance_below = boost::math::constants::pi<double>()*0.5;
wrist_constraint.weight = 1.0;
// constraint_msg.joint_constraints.push_back(wrist_constraint);
constraint_msg.name = "path_constraints";
group.setPathConstraints(constraint_msg);
// group.setStartState(current_state);
// group.setStartStateToCurrentState();
// group.setWorkspace(0,0,0.1,1.0,1.0,1.0);
move_group_interface::MoveGroup::Plan newPlan;
group.plan(newPlan);
// group.execute(newPlan);
ros::waitForShutdown();
}
void testSimple()
{
ros::NodeHandle nh;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
ros::Publisher pub_state = nh.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 20);
ros::Publisher pub_scene = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
sleep(1);
std::vector<shapes::ShapeConstPtr> attached_shapes(1, shapes::ShapeConstPtr(new shapes::Box(0.2, 0.1, 0.1)));
Eigen::Affine3d t;
t.setIdentity();
std::vector<Eigen::Affine3d> attached_poses(1, t);
std::vector<std::string> touch;
touch.push_back("r_wrist_roll_link");
touch.push_back("r_forearm_link");
touch.push_back("r_gripper_palm_link");
touch.push_back("r_gripper_l_finger_link");
touch.push_back("r_gripper_r_finger_link");
scene->getCurrentState().getLinkState("r_wrist_roll_link")->attachBody("attached", attached_shapes,
attached_poses, touch);
collision_detection::CollisionRequest req;
req.verbose = true;
unsigned int N = 2;
for (unsigned int i = 0 ; i < N ; ++i)
{
collision_detection::CollisionResult res;
do
{
ROS_INFO("Trying new state...");
res.collision = false;
if (i + 1 == N)
scene->getCurrentState().setToDefaultValues();
else
scene->getCurrentState().setToRandomValues();
scene->checkSelfCollision(req, res);
}
while (res.collision);
ROS_INFO("Displaying valid state...");
moveit_msgs::PlanningScene psmsg;
scene->getPlanningSceneMsg(psmsg);
pub_scene.publish(psmsg);
ros::Duration(0.5).sleep();
/*
moveit_msgs::DisplayTrajectory d;
d.model_id = scene->getRobotModel()->getName();
planning_models::robotStateToRobotStateMsg(scene->getCurrentState(), d.robot_state);
pub_state.publish(d);
for (int j = 0 ; j < 10 ; ++j)
{
// ros::spinOnce();
ros::Duration(0.01).sleep();
}
*/
}
sleep(1);
planning_scene::PlanningScenePtr colliding = planning_scene::PlanningScene::clone(scene);
// construct a planning scene with 100 objects and no collisions
random_numbers::RandomNumberGenerator rng;
req.verbose = false;
for (int i = 0 ; i < 100000 ; ++i)
{
t.translation() = Eigen::Vector3d(rng.uniformReal(-1, 1), rng.uniformReal(-1, 1), rng.uniformReal(0, 2));
scene->getWorldNonConst()->clearObjects();
scene->getWorldNonConst()->addToObject("spere1", shapes::ShapeConstPtr(new shapes::Sphere(0.05)), t);
collision_detection::CollisionResult res;
scene->checkCollision(req, res);
if (!res.collision)
{
int x = colliding->getWorld()->getObjectIds().size();
colliding->getWorldNonConst()->addToObject("speres" + boost::lexical_cast<std::string>(x),
shapes::ShapeConstPtr(new shapes::Sphere(0.05)), t);
std::cout << x << "\n";
if (x == 100)
break;
}
}
moveit_msgs::PlanningScene psmsg;
colliding->getPlanningSceneMsg(psmsg);
pub_scene.publish(psmsg);
ros::WallTime start = ros::WallTime::now();
unsigned int M = 1000;
for (unsigned int i = 0 ; i < M ; ++i)
{
collision_detection::CollisionResult res;
colliding->checkCollision(req, res);
if (res.collision)
ROS_ERROR("PROBLEM");
}
ROS_INFO("%lf full-collision checks per second", (double)M / (ros::WallTime::now() - start).toSec());
/*
req.verbose = false;
ros::WallTime start = ros::WallTime::now();
// unsigned int N = 50000;
N = 50000;
for (unsigned int i = 0 ; i < N ; ++i)
{
collision_detection::CollisionResult res;
scene->getCurrentState().setToRandomValues();
scene->checkSelfCollision(req, res);
}
ROS_INFO("%lf self-collision checks per second", (double)N / (ros::WallTime::now() - start).toSec());
*/
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "moveit_benchmarks", ros::init_options::AnonymousName);
ros::AsyncSpinner spinner(1);
spinner.start();
boost::program_options::options_description desc;
desc.add_options()
("help", "Show help message")
("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.")
("benchmark-goal-existance", "Benchmark the sampling of the goal region")
("benchmark-planners", "Benchmark only the planners");
boost::program_options::variables_map vm;
boost::program_options::parsed_options po = boost::program_options::parse_command_line(argc, argv, desc);
boost::program_options::store(po, vm);
boost::program_options::notify(vm);
if (vm.count("help") || argc == 1) // show help if no parameters passed
{
std::cout << desc << std::endl;
return 1;
}
try
{
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
moveit_benchmarks::BenchmarkType btype = 0;
moveit_benchmarks::BenchmarkExecution be(psm.getPlanningScene(),
vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
if (vm.count("benchmark-planners"))
btype += moveit_benchmarks::BENCHMARK_PLANNERS;
if (vm.count("benchmark-goal-existance"))
btype += moveit_benchmarks::BENCHMARK_GOAL_EXISTANCE;
unsigned int proc = 0;
std::vector<std::string> files = boost::program_options::collect_unrecognized(po.options, boost::program_options::include_positional);
for (std::size_t i = 0 ; i < files.size() ; ++i)
{
if (be.readOptions(files[i].c_str()))
{
std::stringstream ss;
be.printOptions(ss);
std::cout << "Calling benchmark with options:" << std::endl << ss.str() << std::endl;
be.runAllBenchmarks(btype);
proc++;
}
}
ROS_INFO_STREAM("Processed " << proc << " benchmark configuration files");
}
catch(mongo_ros::DbConnectException &ex)
{
ROS_ERROR_STREAM("Unable to connect to warehouse. If you just created the database, it could take a while for initial setup. Please try to run the benchmark again."
<< std::endl << ex.what());
}
ROS_INFO("Benchmarks complete! Shutting down ROS..."); // because sometimes there are segfaults after this
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "initialize_demo_db", ros::init_options::AnonymousName);
boost::program_options::options_description desc;
desc.add_options()
("help", "Show help message")
("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
boost::program_options::variables_map vm;
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
boost::program_options::notify(vm);
if (vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
if (!psm.getPlanningScene())
{
ROS_ERROR("Unable to initialize PlanningSceneMonitor");
return 1;
}
moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
vm.count("port") ? vm["port"].as<std::size_t>() : 0);
// add default planning scenes
moveit_msgs::PlanningScene psmsg;
psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
psmsg.name = "default1";
pss.addPlanningScene(psmsg);
psmsg.name = "default2";
pss.addPlanningScene(psmsg);
moveit_msgs::RobotState rsmsg;
robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg);
rs.addRobotState(rsmsg, "S1");
rs.addRobotState(rsmsg, "S2");
const std::vector<std::string> &gnames = psm.getRobotModel()->getJointModelGroupNames();
if (gnames.empty())
{
moveit_msgs::Constraints cmsg = kinematic_constraints::constructGoalConstraints(psm.getPlanningScene()->getCurrentState().getJointStateGroup(gnames.front()));
cmsg.name = "Constr1";
cs.addConstraints(cmsg);
cmsg = kinematic_constraints::constructGoalConstraints(psm.getPlanningScene()->getCurrentState().getJointStateGroup(gnames.back()));
cmsg.name = "Constr2";
cs.addConstraints(cmsg);
}
return 0;
}
TEST(OmplPlanning, PathConstrainedSimplePlan)
{
ros::NodeHandle nh;
ros::service::waitForService(PLANNER_SERVICE_NAME);
ros::Publisher pub = nh.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 1);
ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
EXPECT_TRUE(planning_service_client.exists());
EXPECT_TRUE(planning_service_client.isValid());
moveit_msgs::GetMotionPlan::Request mplan_req;
moveit_msgs::GetMotionPlan::Response mplan_res;
planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
planning_scene::PlanningScene &scene = *psm.getPlanningScene();
EXPECT_TRUE(scene.isConfigured());
mplan_req.motion_plan_request.planner_id = "RRTConnectkConfigDefault";
mplan_req.motion_plan_request.group_name = "right_arm";
mplan_req.motion_plan_request.num_planning_attempts = 1;
mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(15.0);
const std::vector<std::string>& joint_names = scene.getRobotModel()->getJointModelGroup("right_arm")->getJointModelNames();
mplan_req.motion_plan_request.start_state.joint_state.name = joint_names;
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(-1.21044517893021499);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(0.038959594993384528);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(-0.81412902362644646);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(-1.0989597173881371);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(2.3582101183671629);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(-1.993988668449755);
mplan_req.motion_plan_request.start_state.joint_state.position.push_back(-2.2779628049776051);
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "r_wrist_roll_link";
pcm.header.frame_id = psm.getPlanningScene()->getPlanningFrame();
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.5;
pcm.constraint_region.primitive_poses[0].position.y = 0.0;
pcm.constraint_region.primitive_poses[0].position.z = 0.7;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
pcm.constraint_region.primitives[0].dimensions.x = 0.1;
pcm.constraint_region.primitives[0].dimensions.y = 0.1;
pcm.constraint_region.primitives[0].dimensions.z = 0.1;
pcm.weight = 1.0;
mplan_req.motion_plan_request.goal_constraints.resize(1);
mplan_req.motion_plan_request.goal_constraints[0].position_constraints.push_back(pcm);
// add path constraints
moveit_msgs::Constraints &constr = mplan_req.motion_plan_request.path_constraints;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = psm.getPlanningScene()->getPlanningFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.15;
ocm.absolute_y_axis_tolerance = 0.15;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
std::cout << mplan_req.motion_plan_request << std::endl;
ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
moveit_msgs::DisplayTrajectory d;
d.model_id = scene.getRobotModel()->getName();
d.trajectory_start = mplan_res.trajectory_start;
d.trajectory = mplan_res.trajectory;
pub.publish(d);
ros::Duration(0.5).sleep();
}
