Commits

Anonymous committed 48d35fc Merge

Merged from dev

Comments (0)

Files changed (23)

Media/models/CMakeLists.txt

File contents unchanged.
 
 
 - Nate Koenig
+

gazebo/common/Image.cc

File contents unchanged.

gazebo/gui/Gui.cc

File contents unchanged.

gazebo/gui/InsertModelWidget.cc

     this->fileTreeWidget->addTopLevelItem(topItem);
 
     boost::filesystem::path dir(path);
-    boost::filesystem::directory_iterator endIter;
     std::list<boost::filesystem::path> resultSet;
 
     if (boost::filesystem::exists(dir) &&
         boost::filesystem::is_directory(dir))
     {
+      std::vector<boost::filesystem::path> paths;
+      std::copy(boost::filesystem::directory_iterator(dir),
+                boost::filesystem::directory_iterator(),
+                std::back_inserter(paths));
+      std::sort(paths.begin(), paths.end());
+
       // Iterate over all the model in the current gazebo path
-      for (boost::filesystem::directory_iterator dirIter(dir);
-          dirIter != endIter; ++dirIter)
+      for (std::vector<boost::filesystem::path>::iterator dIter = paths.begin();
+          dIter != paths.end(); ++dIter)
       {
-        if (boost::filesystem::is_regular_file(dirIter->status()))
+        if (boost::filesystem::is_regular_file(*dIter))
         {
           // This is for boost::filesystem version 3+
-          // std::string modelName = dirIter->path().filename().string();
-          std::string modelName = dirIter->path().filename();
+          // std::string modelName = dIter->path().filename().string();
+          std::string modelName = dIter->filename();
 
           if (modelName.find(".model") != std::string::npos)
           {
             // Add a child item for the model
             QTreeWidgetItem *childItem = new QTreeWidgetItem(topItem,
                 QStringList(QString("%1").arg(
-                    QString::fromStdString(dirIter->path().filename()))));
+                    QString::fromStdString(dIter->filename()))));
             this->fileTreeWidget->addTopLevelItem(childItem);
           }
         }

gazebo/gui/LightListWidget.hh

File contents unchanged.

gazebo/gui/LightRightMenu.hh

File contents unchanged.

gazebo/gui/MainWindow.cc

 
 extern bool g_fullscreen;
 
-
 /////////////////////////////////////////////////
 MainWindow::MainWindow()
   : renderWidget(0)

gazebo/gui/MainWindow.hh

       private slots: void ViewFullScreen();
       private slots: void ViewFPS();
       private slots: void ViewOrbit();
+      private slots: void OnResetModelPoses();
       private slots: void OnResetWorld();
 
       private: void OnFullScreen(bool _value);
       private: QAction *quitAct;
 
       private: QAction *newModelAct;
+      private: QAction *resetModelsAct;
       private: QAction *resetWorldAct;
       private: QAction *editWorldPropertiesAct;
 

gazebo/gui/TimePanel.cc

 void TimePanel::OnTimeReset()
 {
   msgs::WorldControl msg;
-  msg.set_reset_time(true);
+  msg.mutable_reset()->set_time(true);
   this->worldControlPub->Publish(msg);
 }

gazebo/msgs/CMakeLists.txt

File contents unchanged.

gazebo/msgs/world_control.proto

 /// \verbatim
 
 import "header.proto";
+import "world_reset.proto";
 
 message WorldControl
 {
   optional bool pause           = 1;
   optional bool step            = 2;
-  optional bool reset_time      = 3;
-  optional bool reset_world     = 4;
+  optional WorldReset reset     = 3;
 }
 
 /// \endverbatim

gazebo/physics/Base.cc

 //////////////////////////////////////////////////
 void Base::Reset()
 {
+  // this->Reset(BASE);
+}
+
+//////////////////////////////////////////////////
+void Base::Reset(Base::EntityType _resetType)
+{
   Base_V::iterator iter;
   for (iter = this->children.begin(); iter != this->childrenEnd; ++iter)
   {
-    (*iter)->Reset();
+    if ((*iter)->HasType(_resetType))
+      (*iter)->Reset();
+
+    (*iter)->Reset(_resetType);
   }
 }
 

gazebo/physics/Base.hh

 
       public: virtual void Init() {}
       public: virtual void Reset();
+      public: virtual void Reset(Base::EntityType _resetType);
       public: virtual void Update() {}
 
       /// \brief Update the parameters using new sdf values

gazebo/physics/Entity.cc

 void Entity::Reset()
 {
   this->GetWorld()->setWorldPoseMutex->lock();
-  Base::Reset();
 
   if (this->HasType(Base::MODEL))
     this->SetWorldPose(this->initialRelativePose);

gazebo/physics/Joint.cc

 {
   this->SetMaxForce(0, 0);
   this->SetVelocity(0, 0);
-  this->Init();
   this->staticAngle.SetFromRadian(0);
 }
 

gazebo/physics/World.cc

 
   this->name = _name;
 
+  this->resetModelPoses = false;
+  this->resetTime = false;
+
   this->setWorldPoseMutex = new boost::recursive_mutex();
   this->worldUpdateMutex = new boost::recursive_mutex();
 
 {
   if (this->needsReset)
   {
-    this->Reset();
+    if (this->resetModelPoses)
+      this->Reset(this->resetTime, Base::MODEL);
+    else
+      this->Reset(this->resetTime);
+
     this->needsReset = false;
   }
 
 }
 
 //////////////////////////////////////////////////
-void World::Reset(bool _resetTime)
+void World::Reset(bool _resetTime, Base::EntityType _resetType)
 {
+  bool currently_paused = this->IsPaused();
+  this->SetPaused(true);
+  this->worldUpdateMutex->lock();
   if (_resetTime)
   {
     this->simTime = common::Time(0);
     this->startTime = common::Time::GetWallTime();
   }
 
-  this->rootElement->Reset();
+  this->rootElement->Reset(_resetType);
 
   for (std::vector<WorldPluginPtr>::iterator iter = this->plugins.begin();
        iter != this->plugins.end(); ++iter)
     (*iter)->Reset();
   }
   this->physicsEngine->Reset();
+  this->worldUpdateMutex->unlock();
+  this->SetPaused(currently_paused);
 }
 
 //////////////////////////////////////////////////
   if (_data->has_step())
     this->OnStep();
 
-  if (_data->has_reset_time())
-  {
-    this->simTime = common::Time(0);
-    this->pauseTime = common::Time(0);
-    this->startTime = common::Time::GetWallTime();
-  }
-
-  if (_data->has_reset_world())
+  if (_data->has_reset())
   {
     this->needsReset = true;
+
+    if (_data->reset().has_time() && _data->reset().time())
+      this->resetTime = true;
+    else
+      this->resetTime = false;
+
+    if (_data->reset().has_model_poses() &&
+        _data->reset().model_poses())
+      this->resetModelPoses = true;
+    else
+      this->resetModelPoses = false;
   }
 }
 

gazebo/physics/World.hh

 #include "common/CommonTypes.hh"
 #include "common/Event.hh"
 
+#include "physics/Base.hh"
 #include "physics/PhysicsTypes.hh"
 #include "physics/WorldState.hh"
 #include "sdf/sdf.hh"
       public: ModelPtr GetModel(unsigned int index);
 
       /// \brief Reset the simulation to the initial settings
-      public: void Reset(bool _resetTime = true);
+      public: void Reset(bool _resetTime = true,
+        Base::EntityType _resetType = Base::BASE);
 
       /// \brief Get the selected entity
       public: EntityPtr GetSelectedEntity() const;
       private: std::list<msgs::Model> modelMsgs;
 
       private: bool needsReset;
+      private: unsigned int resetModelPoses;
+      private: bool resetTime;
       private: bool initialized;
 
       private: RayShapePtr testRay;

gazebo/sdf/interface/parser.cc

     if (elem->GetName() == "link")
     {
       std::string elemName = elem->GetValueString("name");
-      std::string newName =  modelName + "__" + elemName;
+      std::string newName =  modelName + "::" + elemName;
       replace[elemName] = newName;
       if (elem->HasElementDescription("origin"))
       {
       // for joints, we need to
       //   prefix name like we did with links, and
       std::string elemName = elem->GetValueString("name");
-      std::string newName =  modelName + "__" + elemName;
+      std::string newName =  modelName + "::" + elemName;
       replace[elemName] = newName;
       //   rotate the joint axis because they are model-global
       if (elem->HasElement("axis"))

gazebo/sdf/interface/parser_deprecated.cc

         sdfCollision->GetElement("origin")->GetValuePose("pose");
       gazebo::math::Pose vis_pose =
         sdfVisual->GetElement("origin")->GetValuePose("pose");
-      vis_pose = col_pose*vis_pose;
+
+      // aggregate poses
+      vis_pose.pos = col_pose.pos +
+        col_pose.rot.RotateVector(vis_pose.pos);
+      vis_pose.rot = col_pose.rot * vis_pose.rot;
+
       // update the sdf pose
       sdfVisual->GetElement("origin")->GetAttribute("pose")->Set(vis_pose);
     }

sdf/models/pr2_gripper.model

       <collision name='r_wrist_roll_link_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/forearm_v0/wrist_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/forearm_v0/wrist_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <surface>
           <friction>
       <visual name='r_wrist_roll_link_geom_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/forearm_v0/wrist_roll.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/forearm_v0/wrist_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <material script='PR2/RollLinks'/>
       </visual>
       <visual name='r_wrist_roll_link_geom_r_gripper_palm_link_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/gripper_palm.dae' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/gripper_palm.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
     </link>
       <collision name='r_gripper_l_finger_link_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <surface>
           <friction>
       <visual name='r_gripper_l_finger_link_geom_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
     </link>
       <collision name='r_gripper_l_finger_tip_link_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <surface>
           <friction>
       <visual name='r_gripper_l_finger_tip_link_geom_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
       <sensor name='r_gripper_l_finger_tip_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <contact>
           <collision name='r_gripper_l_finger_tip_link_geom'/>
         </contact>
-        <plugin name='r_gripper_l_finger_tip_gazebo_ros_bumper_controller' filename='libgazebo_ros_bumper.so'>
-          <alwaysOn>1</alwaysOn>
-          <frameName>r_gripper_l_finger_tip_link</frameName>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
-        </plugin>
       </sensor>
     </link>
     <link name='r_gripper_motor_slider_link' gravity='1' self_collide='0' kinematic='0'>
       <collision name='r_gripper_r_finger_link_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <surface>
           <friction>
       <visual name='r_gripper_r_finger_link_geom_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
     </link>
       <collision name='r_gripper_r_finger_tip_link_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
         <surface>
           <friction>
       <visual name='r_gripper_r_finger_tip_link_geom_visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000'/>
         <geometry>
-          <mesh filename='pr2_gripper_meshes/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
+          <mesh filename='pr2/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
       <sensor name='r_gripper_r_finger_tip_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <contact>
           <collision name='r_gripper_r_finger_tip_link_geom'/>
         </contact>
-        <plugin name='r_gripper_r_finger_tip_gazebo_ros_bumper_controller' filename='libgazebo_ros_bumper.so'>
-          <alwaysOn>1</alwaysOn>
-          <frameName>r_gripper_r_finger_tip_link</frameName>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
-        </plugin>
       </sensor>
     </link>
     <link name='torso_lift_motor_screw_link' gravity='1' self_collide='0' kinematic='0'>
         <limit lower='-10000000000000000.000000' upper='10000000000000000.000000' effort='0.000000' velocity='0.000000'/>
       </axis>
     </joint>
-    <plugin name='p3d_r_gripper_l_finger_controller' filename='libgazebo_ros_p3d.so'>
-      <alwaysOn>1</alwaysOn>
-      <updateRate>100.0</updateRate>
-      <bodyName>r_gripper_l_finger_link</bodyName>
-      <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
-      <gaussianNoise>0.0</gaussianNoise>
-      <frameName>base_link</frameName>
-      <xyz>0 0 0</xyz>
-      <rpy>0 -0 0</rpy>
-    </plugin>
-    <plugin name='f3d_r_gripper_l_finger_controller' filename='libgazebo_ros_f3d.so'>
-      <alwaysOn>1</alwaysOn>
-      <updateRate>100.0</updateRate>
-      <bodyName>r_gripper_l_finger_link</bodyName>
-      <topicName>r_gripper_l_finger_force_ground_truth</topicName>
-      <frameName>r_gripper_l_finger_link</frameName>
-    </plugin>
-    <plugin name='p3d_r_gripper_palm_controller' filename='libgazebo_ros_p3d.so'>
-      <alwaysOn>1</alwaysOn>
-      <updateRate>100.0</updateRate>
-      <topicName>r_gripper_palm_pose_ground_truth</topicName>
-      <xyzOffsets>0 0 0</xyzOffsets>
-      <rpyOffsets>0 0 0</rpyOffsets>
-      <gaussianNoise>0.0</gaussianNoise>
-      <frameName>map</frameName>
-      <xyz>0 0 0</xyz>
-      <rpy>0 -0 0</rpy>
-      <bodyName>r_wrist_roll_link</bodyName>
-    </plugin>
   </model>
 </gazebo>

sdf/models/simple_arm.model

     <include filename="pr2_gripper.model" model_pose="0.81 0.188 1.795 0 1.57079633 0"/>
     <joint name='arm_gripper_joint' type='revolute'>
       <parent link='arm_wrist_roll'/>
-      <child link='pr2_gripper__r_wrist_roll_link'/>
+      <child link='pr2_gripper::r_wrist_roll_link'/>
       <axis xyz='0 0 1'>
         <limit lower='0' upper='0'/>
       </axis>

sdf/worlds/stacks.world

           <geometry>
             <sphere radius="0.5"/>
           </geometry>
+          <surface>
+            <contact>
+              <ode soft_erp="1.0" max_vel="0"/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">
           <geometry>
             <sphere radius="0.5"/>
           </geometry>
+          <surface>
+            <contact>
+              <ode soft_erp="1.0" max_vel="0"/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">
           <geometry>
             <sphere radius="0.5"/>
           </geometry>
+          <surface>
+            <contact>
+              <ode soft_erp="1.0" max_vel="0"/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">
           <geometry>
             <sphere radius="0.5"/>
           </geometry>
+          <surface>
+            <contact>
+              <ode soft_erp="1.0" max_vel="0"/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">
           <geometry>
             <sphere radius="0.5"/>
           </geometry>
+          <surface>
+            <contact>
+              <ode soft_erp="1.0" max_vel="0"/>
+            </contact>
+          </surface>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+          <material script="Gazebo/WoodPallet"/>
+        </visual>
+      </link>
+    </model>
+
+    <model name="sphere_1a">
+      <origin pose="1.5 1.5 0.5 0 0 0"/>
+      <link name="link">
+        <inertial mass="1.0">
+          <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+        </inertial>
+
+        <collision name="collision">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+          <material script="Gazebo/WoodPallet"/>
+        </visual>
+      </link>
+    </model>
+    <model name="sphere_2a">
+      <origin pose="1.5 1.5 1.5 0 0 0"/>
+      <link name="link">
+        <inertial mass="1.0">
+          <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+        </inertial>
+
+        <collision name="collision">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+          <material script="Gazebo/WoodPallet"/>
+        </visual>
+      </link>
+    </model>
+    <model name="sphere_3a">
+      <origin pose="1.5 1.5 2.5 0 0 0"/>
+      <link name="link">
+        <inertial mass="1.0">
+          <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+        </inertial>
+
+        <collision name="collision">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+          <material script="Gazebo/WoodPallet"/>
+        </visual>
+      </link>
+    </model>
+    <model name="sphere_4a">
+      <origin pose="1.5 1.5 3.5 0 0 0"/>
+      <link name="link">
+        <inertial mass="1.0">
+          <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+        </inertial>
+
+        <collision name="collision">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
+          <material script="Gazebo/WoodPallet"/>
+        </visual>
+      </link>
+    </model>
+    <model name="sphere_5a">
+      <origin pose="1.5 1.5 4.5 0 0 0"/>
+      <link name="link">
+        <inertial mass="10000.0">
+          <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+        </inertial>
+
+        <collision name="collision">
+          <geometry>
+            <sphere radius="0.5"/>
+          </geometry>
         </collision>
 
         <visual name="visual">