PluralFormat::PluralFormat(const PluralFormat& other)
: Format(other),
locale(other.locale),
msgPattern(other.msgPattern),
numberFormat(NULL),
offset(other.offset) {
copyObjects(other);
}
PluralFormat&
PluralFormat::operator=(const PluralFormat& other) {
if (this != &other) {
locale = other.locale;
msgPattern = other.msgPattern;
offset = other.offset;
copyObjects(other);
}
return *this;
}
label TimeClone::copyObjects(const objectRegistry &src,objectRegistry &dst)
{
Dbug << "Copying stuff from " << src.name() << " to " << dst.name() << endl;
Dbug << "t=" << src.time().timeName() << endl;
Dbug << "Dst AUTO_WRITE: " << (dst.writeOpt()==IOobject::AUTO_WRITE) << endl;
label cnt=0;
forAllConstIter(objectRegistry,src,it) {
const word &name=it.key();
const regIOobject &obj=*(*it);
Dbug << name << " is class " << obj.headerClassName() << endl;
if(isA<objectRegistry>(obj)) {
Dbug << name << " is objectRegistry. Creating new and cloning" << endl;
const objectRegistry &orig=dynamicCast<const objectRegistry>(obj);
if(&src==&orig) {
Dbug << name << "==" << src.name() << " -> Skipping" << endl;
} else {
word dbName=orig.name();
if(dbName==polyMesh::defaultRegion) {
dbName="";
}
autoPtr<objectRegistry> newSubp(
new objectRegistry(
IOobject(
dbName,
src.time().timeName(),
src.local(),
dst
)
)
);
objectRegistry &newSub=newSubp();
Dbug << "AUTO_WRITE: " << (newSub.writeOpt()==IOobject::AUTO_WRITE) << endl;
Dbug << "old path: " << obj.objectPath() << endl;
Dbug << "new Path: " << newSub.objectPath() << endl;
Dbug << "Created registry owned by parent: " << newSub.ownedByRegistry() << endl;
cnt+=copyObjects(orig,newSub);
dst.store(newSubp.ptr());
Dbug << "New registry owned by parent: " << newSub.ownedByRegistry() << endl;
}
} else if(obj.writeOpt()==IOobject::AUTO_WRITE) {
Dbug << name << " set to AUTO_WRITE. Creating copy" << endl;
autoPtr<regIOobject> newObjP;
// work around because there is no virtual clone method in IObobject
#define tryClone(Type) \
if(!newObjP.valid() && isA<Type>(obj)) { \
newObjP.set( \
new Type( \
IOobject( \
obj.name(), \
src.time().timeName(), \
obj.local(), \
dst \
), \
dynamicCast<const Type>(obj))); \
}
tryClone(volScalarField);
tryClone(volVectorField);
tryClone(volTensorField);
tryClone(volSymmTensorField);
tryClone(volSphericalTensorField);
tryClone(surfaceScalarField);
tryClone(surfaceVectorField);
tryClone(surfaceTensorField);
tryClone(surfaceSymmTensorField);
tryClone(surfaceSphericalTensorField);
tryClone(pointScalarField);
tryClone(pointVectorField);
tryClone(pointTensorField);
tryClone(pointSymmTensorField);
tryClone(pointSphericalTensorField);
tryClone(diagTensorIOField);
tryClone(labelIOField);
tryClone(pointIOField);
tryClone(scalarIOField);
tryClone(sphericalTensorIOField);
tryClone(symmTensorIOField);
tryClone(tensorIOField);
tryClone(vector2DIOField);
tryClone(vectorIOField);
// tryClone(polyBoundaryMesh);
#undef tryClone
if(newObjP.valid()) {
regIOobject &newObj=dynamicCast<regIOobject&>(newObjP());
Dbug << "Adding " << name << " to registry " << dst.name()
<< " Class: " << newObj.headerClassName() << endl;
Dbug << "Owned by old Registry: " << newObj.ownedByRegistry() << endl;
Dbug << "AUTO_WRITE: " << (newObj.writeOpt()==IOobject::AUTO_WRITE) << endl;
Dbug << "Old Path: " << obj.objectPath() << endl;
Dbug << "New Path: " << newObj.objectPath() << endl;
Dbug << "Local: " << obj.local() << " -> " << newObj.local() << endl;
newObj.writeOpt()=IOobject::AUTO_WRITE;
regIOobject *ptr=static_cast<regIOobject*>(newObjP.ptr());
dst.store(ptr);
Dbug << "Owned by new Registry: " << newObj.ownedByRegistry() << endl;
cnt++;
} else {
Dbug << "No fitting type found for " << name << endl;
}
} else {
Dbug << name << " not copied" << endl;
}
}
Dbug << "Copying to " << dst.name() << " ended " << cnt << endl;
Dbug << dst.names() << endl;
return cnt;
}
bool StompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::MotionPlanRequest &req,
moveit_msgs::MotionPlanDetailedResponse &res) const
{
ros::WallTime start_time = ros::WallTime::now();
boost::shared_ptr<StompOptimizationTask> stomp_task;
boost::shared_ptr<stomp::STOMP> stomp;
int max_rollouts;
STOMP_VERIFY(node_handle_.getParam("max_rollouts", max_rollouts));
// prepare the collision checkers
boost::shared_ptr<const collision_detection::CollisionRobot> collision_robot; /**< standard robot collision checker */
boost::shared_ptr<const collision_detection::CollisionWorld> collision_world; /**< standard robot -> world collision checker */
boost::shared_ptr<collision_detection::CollisionRobotDistanceField> collision_robot_df; /**< distance field robot collision checker */
boost::shared_ptr<collision_detection::CollisionWorldDistanceField> collision_world_df; /**< distance field robot -> world collision checker */
collision_robot = planning_scene->getCollisionRobot();
collision_world = planning_scene->getCollisionWorld();
std::map<std::string, std::vector<collision_detection::CollisionSphere> > link_body_decompositions;
bool use_signed_distance_field = true;
double padding = 0.0;
double scale = 1.0;
// TODO: remove this disgusting hack!
link_body_decompositions["r_shoulder_pan_link"] = std::vector<collision_detection::CollisionSphere>();
link_body_decompositions["r_shoulder_lift_link"] = std::vector<collision_detection::CollisionSphere>();
collision_robot_df.reset(new collision_detection::CollisionRobotDistanceField(kinematic_model_,
link_body_decompositions,
df_size_x_, df_size_y_, df_size_z_,
use_signed_distance_field,
df_resolution_, df_collision_tolerance_,
df_max_propagation_distance_,
padding, scale));
collision_world_df.reset(new collision_detection::CollisionWorldDistanceField(df_size_x_, df_size_y_, df_size_z_,
use_signed_distance_field,
df_resolution_, df_collision_tolerance_,
df_max_propagation_distance_));
copyObjects(collision_world, collision_world_df);
// first setup the task
stomp_task.reset(new StompOptimizationTask(node_handle_, req.group_name,
kinematic_model_,
collision_robot, collision_world,
collision_robot_df, collision_world_df));
int num_threads=1;
STOMP_VERIFY(stomp_task->initialize(num_threads, max_rollouts));
XmlRpc::XmlRpcValue features_xml;
STOMP_VERIFY(node_handle_.getParam("features", features_xml));
stomp_task->setFeaturesFromXml(features_xml);
stomp_task->setControlCostWeight(0.00001);
stomp_task->setTrajectoryVizPublisher(const_cast<ros::Publisher&>(trajectory_viz_pub_));
stomp_task->setDistanceFieldVizPublisher((const_cast<ros::Publisher&>(trajectory_viz_pub_)));
stomp_task->setRobotBodyVizPublisher((const_cast<ros::Publisher&>(robot_body_viz_pub_)));
stomp_task->setMotionPlanRequest(planning_scene, req);
stomp.reset(new stomp::STOMP());
stomp->initialize(node_handle_, stomp_task);
// TODO: don't hardcode these params
bool success = stomp->runUntilValid(100, 10);
std::vector<Eigen::VectorXd> best_params;
double best_cost;
stomp->getBestNoiselessParameters(best_params, best_cost);
stomp_task->publishTrajectoryMarkers(const_cast<ros::Publisher&>(trajectory_viz_pub_), best_params);
res.trajectory_start = req.start_state;
res.group_name = req.group_name;
res.trajectory.resize(1);
stomp_task->parametersToJointTrajectory(best_params, res.trajectory[0].joint_trajectory);
res.description.resize(1);
res.description[0] = "STOMP";
res.processing_time.resize(1);
ros::WallDuration wd = ros::WallTime::now() - start_time;
res.processing_time[0] = ros::Duration(wd.sec, wd.nsec);
if (!success)
{
ROS_ERROR("STOMP: failed to find a collision-free plan");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return true;
}
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
// res.trajectory
// res.description
// res.processing_time
// res.error_code
return true;
}
Fork::Fork(const Environment::Ptr parentEnv_, const RaveInstancePtr rave_, BulletInstance::Ptr bullet, OSGInstance::Ptr osg) :
parentEnv(parentEnv_.get()), env(new Environment(bullet, osg)),
rave(rave_) {
copyObjects();
}
Fork::Fork(const Environment *parentEnv_, BulletInstance::Ptr bullet, OSGInstance::Ptr osg) :
parentEnv(parentEnv_), env(new Environment(bullet, osg)) {
copyObjects();
}
