update

robotflow-initiative · Aug 5, 2021 · aef77c7 · aef77c7
1 parent ced6551
commit aef77c7
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 42 deletions.
diff --git a/notebook/QuickStart.ipynb b/notebook/QuickStart.ipynb
@@ -52,7 +52,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "723ad7fc",
+   "id": "dce03ab3",
    "metadata": {},
    "source": [
     "### Convert Urdf file\n",
@@ -67,8 +67,9 @@
     "\n",
     "In order to get the urdf file we use here, please run\n",
     "```bash\n",
-    "./urdf_converter ../resources/pr2.urdf <absolute path to resource> ../resources/pr2_convert.urdf\n",
+    "./urdf_converter ../resources/pr2.urdf <absolute path to resource directory> ../resources/pr2_convert.urdf\n",
     "```\n",
+    "**Note:** Do not omit the last `'/'` at the end of absolute path.\n",
     "\n",
     "However, it is ok to use `pr2.urdf` directly. But all of its meshes may not be loaded into moveit which results in unexpected collision."
    ]
@@ -587,7 +588,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "ae991dab",
+   "id": "05a657c9",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -611,7 +612,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "5d1663ce",
+   "id": "804c90ef",
    "metadata": {},
    "source": [
     "### Move Along Trajectory\n",
@@ -636,7 +637,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "4ba9a0dc",
+   "id": "87434e45",
    "metadata": {},
    "source": [
     "Now we can sample trajectory info at any time point (even outside the bound of trajectory start time and end time). We propose many different sample methods for SplineTrajectory, the most important two of them is `sample_by_interval` and `sample_at_time`.\n",
@@ -651,7 +652,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "40bfcfff",
+   "id": "94156a42",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -661,7 +662,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "99e78f29",
+   "id": "b218d9af",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -670,7 +671,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "659a3808",
+   "id": "8fb73e65",
    "metadata": {},
    "source": [
     "Here we use matplotlib to draw the difference between RobotTrajectory and SplineTrajectory."
@@ -679,7 +680,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "0a185f07",
+   "id": "3e138959",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -752,7 +753,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "95a5dee8",
+   "id": "954eed18",
    "metadata": {},
    "source": [
     "### Control robot By Pybullet.setJointMotorArray Directly\n",
@@ -762,7 +763,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "649c631d",
+   "id": "be4b772b",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -785,7 +786,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "03d08ce2",
+   "id": "5396ee43",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -794,7 +795,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "dd49c5b2",
+   "id": "88746ac5",
    "metadata": {},
    "source": [
     "### Control robot By Controller\n",
@@ -806,7 +807,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "72b4a919",
+   "id": "869c2ebf",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -816,7 +817,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "1a1010c1",
+   "id": "8d940841",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -826,7 +827,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "42908a64",
+   "id": "4b406ec4",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -835,7 +836,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "989e3412",
+   "id": "1cd39977",
    "metadata": {},
    "source": [
     "Here we use a small thread to repeat the motion plan and robot controll process."
@@ -844,7 +845,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "9c24a950",
+   "id": "ca06f4bb",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -901,7 +902,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "63a10b5a",
+   "id": "e4365400",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -912,7 +913,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "f5b54d91",
+   "id": "017ed44c",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -923,7 +924,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "5d7a078c",
+   "id": "3262966a",
    "metadata": {},
    "source": [
     "## Add Collision Object\n",
@@ -936,7 +937,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "81a3aefb",
+   "id": "c7f95212",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -958,7 +959,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "4998f077",
+   "id": "e3c34a4d",
    "metadata": {},
    "source": [
     "The box can be removed by `pybullet.removeBody(box_body_id)`."
@@ -967,7 +968,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "2b31fd9c",
+   "id": "407edea2",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -976,7 +977,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "ca842a06",
+   "id": "d76fad82",
    "metadata": {},
    "source": [
     "### Add object in moveit\n",
@@ -988,7 +989,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "4af43658",
+   "id": "190c697e",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -997,7 +998,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "b0c41e6f",
+   "id": "1bc5b91b",
    "metadata": {},
    "source": [
     "We need to create a shape first. Note that the size is not equal to `halfExtents`."
@@ -1006,7 +1007,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "cc0d2fe8",
+   "id": "669b3b5c",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -1015,7 +1016,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "ba009954",
+   "id": "8b71ddb5",
    "metadata": {},
    "source": [
     "Then create an Eigen affine matrix which denotes the pose of this object."
@@ -1024,7 +1025,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "cb1ea42d",
+   "id": "1f694af1",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -1036,7 +1037,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "7b4c8faf",
+   "id": "23c4bef5",
    "metadata": {},
    "source": [
     "Add `moveit_box` to collision world with pose `box_pose`."
@@ -1045,7 +1046,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "71f7485a",
+   "id": "c6c3aebc",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -1055,7 +1056,7 @@
   },
   {
    "cell_type": "markdown",
-   "id": "7b41a339",
+   "id": "7dc87ed8",
    "metadata": {},
    "source": [
     "### Add object through SceneHelper\n",
@@ -1067,17 +1068,17 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "c3dd4c22",
+   "id": "4dbb3fb4",
    "metadata": {},
    "outputs": [],
    "source": [
-    "pybullet.removeBody(box_body_id)\n",
-    "world.removeObject(\"box_0\")"
+    "#pybullet.remoeBody(box_body_id)\n",
+    "#world.removeObject(\"box_0\")"
    ]
   },
   {
    "cell_type": "markdown",
-   "id": "06a36797",
+   "id": "dd913ba6",
    "metadata": {},
    "source": [
     "Then we add it through one single method of SceneHelper."
@@ -1088,13 +1089,13 @@
    "id": "630a14b3",
    "metadata": {},
    "source": [
-    "**To Be Continued ...**"
+    "### Test Obstacle Avoidance"
    ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "805de650",
+   "id": "4f512cd7",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -1105,7 +1106,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "890fe416",
+   "id": "cc2626c3",
    "metadata": {},
    "outputs": [],
    "source": [
@@ -1117,7 +1118,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "d4027e92",
+   "id": "35584f8d",
    "metadata": {},
    "outputs": [],
    "source": []

diff --git a/src/python/include/hardware_pybullet.h b/src/python/include/hardware_pybullet.h
@@ -46,6 +46,8 @@ class PybulletJointHandler : public JointHandler {
 
 class PybulletHardware : public HardwareInterface {
 public:
+    static int GEOM_BOX_;
+
     typedef py::detail::accessor<py::detail::accessor_policies::str_attr> PyAttr;
     explicit PybulletHardware(py::handle pybullet, int bodyUniqueId);
     int getNumBodies();

diff --git a/src/python/include/helper_pybullet.h b/src/python/include/helper_pybullet.h
@@ -1,5 +1,5 @@
 /**
- * @details Some help methods for pybullet. However, many of these methods can be implemented as member function
+ * @details Some helper methods for pybullet. However, many of these methods can be implemented as member function
  * of PybulletHardware. But I do not hope that the hardware itself depends on moveit.
  * So these methods are implemented as helper functions with PybulletHardware as parameter.
  */
@@ -52,6 +52,9 @@ class PlanningSceneHelper{
       */
      Eigen::Affine3d linkRelativeTransform(const std::string& link_name, const std::string& frame, bool _sync = true);
 
+     //int addBox(const std::string& object_id, std::vector<double> size, std::vector<double> position,
+     //           );
+
 private:
     PybulletHardware pybullet_; // hardware only contains accessors.
                                 // It is ok to keep an instance instead of an reference.

diff --git a/src/python/src/hardware_pybullet.cpp b/src/python/src/hardware_pybullet.cpp
@@ -11,6 +11,8 @@ int PybulletJointHandler::POSITION_CONTROL_; // definition
 int PybulletJointHandler::VELOCITY_CONTROL_; // definition
 int PybulletJointHandler::TORQUE_CONTROL_; // definition
 
+int PybulletHardware::GEOM_BOX_;
+
 PybulletJointHandler::PybulletJointHandler(int body_id,
                                            int joint_index,
                                            const std::string& joint_name,
@@ -72,6 +74,9 @@ PybulletHardware::PybulletHardware(py::handle pybullet, int bodyUniqueId)
 
     PybulletJointHandler::POSITION_CONTROL_ = pybullet_.attr("POSITION_CONTROL").cast<int>();
     PybulletJointHandler::VELOCITY_CONTROL_ = pybullet_.attr("VELOCITY_CONTROL").cast<int>();
+
+    GEOM_BOX_ = pybullet_.attr("GEOM_BOX").cast<int>();
+
     std::cout << "Create PybulletHardware for body " << body_id_ << " with " << joint_num_ << " joints." << std::endl;
     all_index_.resize(joint_num_);
     joint_names_.resize(joint_num_);