initial commit, script for fullBody not up to date

.gitignore

+# Compiled Object files
+*.slo
+*.lo
+*.o
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+*.pyc
+*.~
+
+# Executables
+*.exe
+*.out
+*.app
+
+build/
+
+CMakeLists.txt.user

.gitmodules

+[submodule "cmake"]
+	path = cmake
+	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

+# Copyright (c) 2012 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-rbprm-corba is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-rbprm-corba.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+# Requires at least CMake 2.6 to configure the package.
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+SET(CXX_DISABLE_WERROR true)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/idl.cmake)
+INCLUDE(cmake/python.cmake)
+
+SET(PROJECT_NAME anymal-rbprm)
+SET(PROJECT_DESCRIPTION "Database for anymal robot using hpp-rbprm")
+SET(PROJECT_URL "")
+
+SET(CUSTOM_HEADER_DIR "hpp/anymal-rbprm")
+
+SETUP_PROJECT ()
+
+# search for python
+FINDPYTHON(2.7 EXACT REQUIRED)
+
+PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
+
+SET(CATKIN_PACKAGE_SHARE_DESTINATION
+  ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
+  
+  
+SET(CATKIN_PACKAGE_SHARE_DESTINATION
+  ${CMAKE_INSTALL_DATAROOTDIR}/anymal-rbprm)
+  
+  
+install(DIRECTORY
+  data
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+  
+  
+install (FILES
+  src/hpp/corbaserver/rbprm/anymal/robot.py
+  src/hpp/corbaserver/rbprm/anymal/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/anymal)
+
+
+install (FILES
+  src/hpp/corbaserver/rbprm/anymal_contact6D/robot.py
+  src/hpp/corbaserver/rbprm/anymal_contact6D/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/anymal_contact6D)
+
+install (FILES
+  src/hpp/corbaserver/rbprm/anymal_abstract/robot.py
+  src/hpp/corbaserver/rbprm/anymal_abstract/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/anymal_abstract)
+
+
+SETUP_PROJECT_FINALIZE()
+

README.md

+# anymal-rbprm
+File database for anymal robot using the hpp-rbprm framework

cmake @ ecaf20c2

+Subproject commit ecaf20c28c8ce33de312fbd812f88037cf995b73

data/package.xml

+<package>
+  <name>anymal-rbprm</name>
+  <version>1.0.0</version>
+  <description>
+    Info Database for using rbprm with anymal.
+  </description>
+  <author>Pierre Fernbach</author>
+  <maintainer email="hpp@laas.fr">Pierre Fernbach</maintainer>
+  <build_depend>hpp-rbprm</build_depend>
+</package>

data/urdf/anymal_LFleg_rom.urdf

+<robot name="anymal_LFleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/LFleg_vN_Rom.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/LFleg_vN_Rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

data/urdf/anymal_LHleg_rom.urdf

+<robot name="anymal_LHleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/LHleg_vN_Rom.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/LHleg_vN_Rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

data/urdf/anymal_RFleg_rom.urdf

+<robot name="anymal_RFleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/RFleg_vN_Rom.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/RFleg_vN_Rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

data/urdf/anymal_RHleg_rom.urdf

+<robot name="anymal_RHleg_rom">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/RHleg_vN_Rom.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/RHleg_vN_Rom.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

data/urdf/anymal_trunk.urdf

+<robot name="anymal_all">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/anymal_all_visual.dae"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <!--visual>
+      <origin xyz="0 0 -0.05" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/anymal_trunk.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual-->
+    <collision>
+      <origin xyz="0 0 -0.05" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://anymal-rbprm/meshes/anymal_trunk.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

src/hpp/corbaserver/rbprm/anymal/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2019 CNRS
+# Author : Pierre Fernbach
+
+from robot import Robot

src/hpp/corbaserver/rbprm/anymal/robot.py

+#!/usr/bin/env python
+# Copyright (c) 2019 CNRS
+# Author: Pierre Fernbach
+#
+# This file is part of hpp-rbprm-robot-data.
+# hpp_tutorial is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp_tutorial is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp_tutorial.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
+from pinocchio import SE3, Quaternion
+import numpy as np
+
+class Robot (Parent):
+    ##
+    #  Information to retrieve urdf and srdf files.
+
+    packageName = "hyq_description"
+    meshPackageName = "hyq_description"
+    rootJointType = "freeflyer"
+    urdfName = "hyq"
+    urdfSuffix = ""
+    srdfSuffix = ""
+
+    ## Information about the names of thes joints defining the limbs of the robot
+    rLegId = 'rfleg'
+    rleg = 'rf_haa_joint'
+    rfoot = 'rf_foot_joint'
+    lLegId = 'lfleg'
+    lleg = 'lf_haa_joint'
+    lfoot = 'lf_foot_joint'
+    lArmId = 'lhleg'
+    larm = 'lh_haa_joint'
+    lhand = 'lh_foot_joint'
+    rArmId = 'rhleg'
+    rarm = 'rh_haa_joint'
+    rhand = 'rh_foot_joint'
+
+
+    referenceConfig =[0.,
+     0.,
+     0.6,
+     0.,
+     0.,
+     0.,
+     1.,
+    0, # LF
+    0.7853981633974483,
+    -1.5707963267948966,
+    0, # LH
+    -0.7853981633974483,
+    1.5707963267948966,
+    0, # RF
+    0.7853981633974483,
+    -1.5707963267948966,
+    0, # RH
+    -0.7853981633974483,
+    1.5707963267948966,
+]
+    
+    # informations required to generate the limbs databases the limbs : 
+
+    offset = [0.,0.,-0.021]
+    normal = [0,0,1]
+    legx = 0.02; legy = 0.02
+    kinematicConstraintsPath="package://hyq-rbprm/com_inequalities/"
+    rLegKinematicConstraints=kinematicConstraintsPath+rleg+"_com_constraints.obj"
+    lLegKinematicConstraints=kinematicConstraintsPath+lleg+"_com_constraints.obj" 
+    rArmKinematicConstraints=kinematicConstraintsPath+rarm+"_com_constraints.obj" 
+    lArmKinematicConstraints=kinematicConstraintsPath+larm+"_com_constraints.obj"
+
+    # data used by scripts :
+    limbs_names = [rLegId,lLegId,rArmId,lArmId]
+    dict_limb_joint = {rLegId:rfoot, lLegId:lfoot, rArmId:rhand, lArmId:lhand}
+    dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1],rhand:[0,0,1,1],lhand:[0.9,0.5,0,1]}
+    FOOT_SAFETY_SIZE = 0.01
+    # size of the contact surface (x,y)
+    dict_size={rfoot:[0.02 , 0.02], lfoot:[0.02 , 0.02],rhand:[0.02 , 0.02],lhand:[0.02 , 0.02]}
+
+    #various offset used by scripts
+    MRsole_offset = SE3.Identity()
+    MRsole_offset.translation = np.matrix(offset).T
+    MLsole_offset = MRsole_offset.copy()
+    MRhand_offset = MRsole_offset.copy()
+    MLhand_offset = MRsole_offset.copy()
+    dict_offset = {rfoot:MRsole_offset, lfoot:MLsole_offset, rhand:MRhand_offset, lhand:MLhand_offset}
+
+    # display transform :
+    MRsole_display = SE3.Identity()
+    MLsole_display = SE3.Identity()
+    MRhand_display = SE3.Identity()
+    MLhand_display = SE3.Identity()
+    dict_display_offset = {rfoot:MRsole_display, lfoot:MLsole_display, rhand:MRhand_display, lhand:MLhand_display}
+
+    def __init__ (self, name = None,load = True):
+        Parent.__init__ (self,load)
+        if load:
+            self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix)
+        if name != None:
+            self.name = name

src/hpp/corbaserver/rbprm/anymal_abstract/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2019 CNRS
+# Author : Pierre Fernbach
+
+from robot import Robot