Merge pull request #9 from cram2/boxy-melodic

Boxy melodic

.gitignore

@@ -2,4 +2,5 @@
 build
 *#
 latest_asdf_check
-.#*
+.#*
+*~

LICENSE

@@ -0,0 +1,30 @@
+
+Copyright (c) 2009 - 2021, CRAM team
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Intelligent Autonomous Systems Group/
+      Technische Universitaet Muenchen, nor the name of the
+      Institute for Artificial Intelligence/Universitaet Bremen,
+      nor the names of their contributors may be used to
+      endorse or promote products derived from this software without
+      specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

cram-18.04.rosinstall

@@ -0,0 +1,21 @@
+# THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2018-10-16
+- git:
+    local-name: cram
+    uri: https://github.com/cram2/cram.git
+    version: master-melodic
+- git:
+    local-name: giskard_msgs
+    uri: https://github.com/SemRoCo/giskard_msgs.git
+    version: 5bfb2a2dc0d58635fab4eceaf93a43c2729a4cdf
+- git:
+    local-name: iai_common_msgs
+    uri: https://github.com/code-iai/iai_common_msgs.git
+    version: 46d6e9b9be386de9cf5e153f489c382e0c66f74c
+- git:
+    local-name: iai_maps
+    uri: https://github.com/code-iai/iai_maps.git
+    version: 497632f5bbf8f028b5cf25470efeefbaaf9ce388
+- git:
+    local-name: genlisp
+    uri: https://github.com/ros/genlisp.git
+    version: 3ac633abacdf5ab321d23ed013c7d5b7da97736d

cram_3d_world/cl_bullet/cl-bullet-examples.asd

@@ -0,0 +1,39 @@
+;;;
+;;; Copyright (c) 2010, Lorenz Moesenlechner <[email protected]>
+;;; All rights reserved.
+;;;
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;;
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen nor the names of its contributors
+;;;       may be used to endorse or promote products derived from this software
+;;;       without specific prior written permission.
+;;;
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+;;;
+
+(defsystem cl-bullet-examples
+  :author "Lorenz Moesenlechner <[email protected]>"
+  :license "BSD"
+  :depends-on (cl-bullet cl-transforms)
+  :components
+  ((:module "src/lisp/examples"
+    :components
+    ((:file "package")
+     (:file "hello-world" :depends-on ("package"))))))

cram_3d_world/cl_bullet/cl-bullet.asd

@@ -0,0 +1,67 @@
+;;;
+;;; Copyright (c) 2010, Lorenz Moesenlechner <[email protected]>
+;;; All rights reserved.
+;;;
+;;; Redistribution and use in source and binary forms, with or without
+;;; modification, are permitted provided that the following conditions are met:
+;;;
+;;;     * Redistributions of source code must retain the above copyright
+;;;       notice, this list of conditions and the following disclaimer.
+;;;     * Redistributions in binary form must reproduce the above copyright
+;;;       notice, this list of conditions and the following disclaimer in the
+;;;       documentation and/or other materials provided with the distribution.
+;;;     * Neither the name of the Intelligent Autonomous Systems Group/
+;;;       Technische Universitaet Muenchen nor the names of its contributors
+;;;       may be used to endorse or promote products derived from this software
+;;;       without specific prior written permission.
+;;;
+;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+;;; POSSIBILITY OF SUCH DAMAGE.
+;;;
+
+(defsystem cl-bullet
+  :author "Lorenz Moesenlechner <[email protected]>"
+  :license "BSD"
+  :description "A common lisp wrapper for the bullet library"
+
+  :depends-on (:cffi
+               :cffi-ros-utils
+               :trivial-garbage
+               :ros-load-manifest
+               :cram-utilities
+               :split-sequence
+               :cl-transforms)
+  :components
+  ((:module "src/lisp"
+    :components
+    ((:file "package")
+     (:file "foreign-types" :depends-on ("package"))
+     (:file "cffi" :depends-on ("package" "foreign-types"))
+     (:file "foreign-class" :depends-on ("package"))
+     (:file "debug-draw" :depends-on ("package" "foreign-types" "cffi" "foreign-class"))
+     (:file "bt-world" :depends-on ("package" "cffi" "foreign-class" "rigid-body" "contact-manifold"))
+     (:file "contact-manifold" :depends-on ("package"))
+     (:file "motion-state" :depends-on ("package" "cffi" "foreign-class"))
+     (:file "rigid-body" :depends-on ("package" "cffi" "foreign-class" "motion-state"))
+     (:file "collision-shapes" :depends-on ("package" "cffi" "foreign-class"))
+     (:file "constraints" :depends-on ("package" "cffi" "foreign-class"))
+     (:file "point-2-point-constraint" :depends-on ("package" "cffi" "constraints"))
+     (:file "hinge-constraint" :depends-on ("package" "cffi" "constraints"))
+     (:file "slider-constraint" :depends-on ("package" "cffi" "constraints"))
+     (:file "world-state" :depends-on ("package"
+                                       "bt-world"
+                                       "rigid-body"
+                                       "collision-shapes"
+                                       "point-2-point-constraint"
+                                       "hinge-constraint"
+                                       "slider-constraint"))))))
+

cram_3d_world/cl_bullet_vis/src/math-function-visualization.lisp

@@ -86,9 +86,12 @@
             (gl:mult-matrix (pose->gl-matrix pose))
             (loop for x from (- (/ width 2)) to (- (/ width 2) step-size) by step-size do
               (loop for y from (- (/ height 2)) to (- (/ height 2) step-size) by step-size do
-                (let ((pt-1 (get-point x y)))
-                  (when pt-1
-                    (let* ((value (cl-transforms:z pt-1))
+                (let ((middle-pt (get-point (+ x (/ step-size 2))
+                                            (+ y (/ step-size 2)))))
+                  (when middle-pt
+                    (let* ((value (cl-transforms:z middle-pt))
+                           (pt-1 (cl-transforms:make-3d-vector
+                                  x y value))
                            (pt-2 (cl-transforms:make-3d-vector
                                   (+ x step-size) y value))
                            (pt-3 (cl-transforms:make-3d-vector