Skip to content
GitLab
Unverified Commit 820d178c authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #3 from pFernbach/master 

update ROMs and script
parents 5349a5de b75e6dc4
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 820d178c
......@@ -51,6 +51,11 @@ install (FILES
src/hpp/corbaserver/rbprm/talos_abstract/__init__.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/talos_abstract)
install (FILES
src/hpp/corbaserver/rbprm/talos_fixedUpper/robot.py
src/hpp/corbaserver/rbprm/talos_fixedUpper/__init__.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/talos_fixedUpper)
install (FILES
src/hpp/corbaserver/rbprm/talos/robot.py
src/hpp/corbaserver/rbprm/talos/__init__.py
......
data/talos/meshes/talos_lleg_rom.dae
View file @ 820d178c
This diff is collapsed.
data/talos/meshes/talos_rleg_rom.dae
View file @ 820d178c
This diff is collapsed.
data/talos/meshes/talos_trunk_lower_large.dae
View file @ 820d178c
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 2.77.0 commit date:2016-04-05, commit time:18:12, hash:abf6f08</authoring_tool>
<authoring_tool>Blender 2.79.0 commit date:2018-03-22, commit time:14:10, hash:f4dc9f9d68b</authoring_tool>
</contributor>
<created>2018-12-06T14:47:36</created>
<modified>2018-12-06T14:47:36</modified>
<created>2019-05-24T10:47:48</created>
<modified>2019-05-24T10:47:48</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
......@@ -26,7 +26,7 @@
<color sid="diffuse">0.64 0 0 1</color>
</diffuse>
<specular>
<color sid="specular">0.00390625 0.00390625 0.00390625 1</color>
<color sid="specular">0.001953125 0.001953125 0.001953125 1</color>
</specular>
<shininess>
<float sid="shininess">50</float>
......@@ -45,46 +45,45 @@
</material>
</library_materials>
<library_geometries>
<geometry id="Talos_Trunk_Lower-mesh" name="Talos Trunk Lower">
<geometry id="Talos_Trunk_Lower_Large_002-mesh" name="Talos Trunk Lower Large.002">
<mesh>
<source id="Talos_Trunk_Lower-mesh-positions">
<float_array id="Talos_Trunk_Lower-mesh-positions-array" count="72">-0.06749182 0.27 -0.3690578 -0.06749182 -0.27 -0.6275106 -0.06749182 -0.27 -0.3690578 0.132508 0.27 -0.3690578 -0.06749182 0.27 -0.6275106 0.132508 -0.27 -0.3690578 0.132508 0.27 -0.6275106 0.132508 -0.27 -0.6275106 -0.1995605 0.5 -0.1799489 -0.1995605 -0.5 -0.3799489 -0.1995605 -0.5 -0.1799489 0.1204394 0.5 -0.1799489 -0.1995605 0.5 -0.3799489 0.1204394 -0.5 -0.1799489 0.1204394 0.5 -0.3799489 0.1204394 -0.5 -0.3799489 -0.1995605 0.25 0.01293671 -0.1995605 -0.25 -0.3870633 -0.1995605 -0.25 0.01293671 0.1204394 0.25 0.01293671 -0.1995605 0.25 -0.3870633 0.1204394 -0.25 0.01293671 0.1204394 0.25 -0.3870633 0.1204394 -0.25 -0.3870633</float_array>
<source id="Talos_Trunk_Lower_Large_002-mesh-positions">
<float_array id="Talos_Trunk_Lower_Large_002-mesh-positions-array" count="72">-0.06749182 0.27 -0.3690578 -0.06749182 -0.27 -0.6775106 -0.06749182 -0.27 -0.3690578 0.132508 0.27 -0.3690578 -0.06749182 0.27 -0.6775106 0.132508 -0.27 -0.3690578 0.132508 0.27 -0.6775106 0.132508 -0.27 -0.6775106 -0.1995605 0.6 -0.1799489 -0.1995605 -0.4 -0.4735262 -0.1995605 -0.6 -0.1799489 0.1204394 0.6 -0.1799489 -0.1995605 0.4 -0.4735262 0.1204394 -0.6 -0.1799489 0.1204394 0.4 -0.4735262 0.1204394 -0.4 -0.4735262 -0.1995605 0.25 0.01293671 -0.1995605 -0.25 -0.3870633 -0.1995605 -0.25 0.01293671 0.1204394 0.25 0.01293671 -0.1995605 0.25 -0.3870633 0.1204394 -0.25 0.01293671 0.1204394 0.25 -0.3870633 0.1204394 -0.25 -0.3870633</float_array>
<technique_common>
<accessor source="#Talos_Trunk_Lower-mesh-positions-array" count="24" stride="3">
<accessor source="#Talos_Trunk_Lower_Large_002-mesh-positions-array" count="24" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Talos_Trunk_Lower-mesh-normals">
<float_array id="Talos_Trunk_Lower-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<source id="Talos_Trunk_Lower_Large_002-mesh-normals">
<float_array id="Talos_Trunk_Lower_Large_002-mesh-normals-array" count="30">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1 0 0.8264452 -0.5630173 0 -0.8264452 -0.5630173 0 0.8264454 -0.5630168 0 -0.8264454 -0.5630168</float_array>
<technique_common>
<accessor source="#Talos_Trunk_Lower-mesh-normals-array" count="6" stride="3">
<accessor source="#Talos_Trunk_Lower_Large_002-mesh-normals-array" count="10" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Talos_Trunk_Lower-mesh-vertices">
<input semantic="POSITION" source="#Talos_Trunk_Lower-mesh-positions"/>
<vertices id="Talos_Trunk_Lower_Large_002-mesh-vertices">
<input semantic="POSITION" source="#Talos_Trunk_Lower_Large_002-mesh-positions"/>
</vertices>
<polylist material="Material_001-material" count="36">
<input semantic="VERTEX" source="#Talos_Trunk_Lower-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Talos_Trunk_Lower-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>0 0 1 0 2 0 3 1 4 1 0 1 5 2 6 2 3 2 2 3 7 3 5 3 4 4 7 4 1 4 3 5 2 5 5 5 8 0 9 0 10 0 11 1 12 1 8 1 13 2 14 2 11 2 10 3 15 3 13 3 12 4 15 4 9 4 11 5 10 5 13 5 16 0 17 0 18 0 19 1 20 1 16 1 21 2 22 2 19 2 18 3 23 3 21 3 20 4 23 4 17 4 19 5 18 5 21 5 0 0 4 0 1 0 3 1 6 1 4 1 5 2 7 2 6 2 2 3 1 3 7 3 4 4 6 4 7 4 3 5 0 5 2 5 8 0 12 0 9 0 11 1 14 1 12 1 13 2 15 2 14 2 10 3 9 3 15 3 12 4 14 4 15 4 11 5 8 5 10 5 16 0 20 0 17 0 19 1 22 1 20 1 21 2 23 2 22 2 18 3 17 3 23 3 20 4 22 4 23 4 19 5 16 5 18 5</p>
</polylist>
<triangles material="Material_001-material" count="36">
<input semantic="VERTEX" source="#Talos_Trunk_Lower_Large_002-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Talos_Trunk_Lower_Large_002-mesh-normals" offset="1"/>
<p>0 0 1 0 2 0 3 1 4 1 0 1 5 2 6 2 3 2 2 3 7 3 5 3 4 4 7 4 1 4 3 5 2 5 5 5 8 0 9 0 10 0 11 6 12 6 8 6 13 2 14 2 11 2 10 7 15 7 13 7 12 4 15 4 9 4 11 5 10 5 13 5 16 0 17 0 18 0 19 1 20 1 16 1 21 2 22 2 19 2 18 3 23 3 21 3 20 4 23 4 17 4 19 5 18 5 21 5 0 0 4 0 1 0 3 1 6 1 4 1 5 2 7 2 6 2 2 3 1 3 7 3 4 4 6 4 7 4 3 5 0 5 2 5 8 0 12 0 9 0 11 8 14 8 12 8 13 2 15 2 14 2 10 9 9 9 15 9 12 4 14 4 15 4 11 5 8 5 10 5 16 0 20 0 17 0 19 1 22 1 20 1 21 2 23 2 22 2 18 3 17 3 23 3 20 4 22 4 23 4 19 5 16 5 18 5</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_controllers/>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="Talos_Trunk_Lower" name="Talos_Trunk_Lower" type="NODE">
<node id="Talos_Trunk_Lower_Large" name="Talos_Trunk_Lower_Large" type="NODE">
<matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
<instance_geometry url="#Talos_Trunk_Lower-mesh" name="Talos_Trunk_Lower">
<instance_geometry url="#Talos_Trunk_Lower_Large_002-mesh" name="Talos_Trunk_Lower_Large">
<bind_material>
<technique_common>
<instance_material symbol="Material_001-material" target="#Material_001-material"/>
......
data/talos/meshes/talos_trunk_lower_visual_large.dae 0 → 100644
View file @ 820d178c
This diff is collapsed.
data/talos/urdf/talos_trunk_large.urdf
View file @ 820d178c
......@@ -43,7 +43,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://talos-rbprm/meshes/talos_trunk_lower_visual.dae" scale="1 1 1"/>
<mesh filename="package://talos-rbprm/meshes/talos_trunk_lower_visual_large.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
......
src/hpp/corbaserver/rbprm/talos/robot.py
View file @ 820d178c
......@@ -171,7 +171,12 @@ class Robot (Parent):
self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix)
if name != None:
self.name = name
self.joint1L_bounds_prev=self.getJointBounds('leg_left_1_joint')
self.joint6L_bounds_prev=self.getJointBounds('leg_left_6_joint')
self.joint2L_bounds_prev=self.getJointBounds('leg_left_2_joint')
self.joint1R_bounds_prev=self.getJointBounds('leg_right_1_joint')
self.joint6R_bounds_prev=self.getJointBounds('leg_right_6_joint')
self.joint2R_bounds_prev=self.getJointBounds('leg_right_2_joint')
def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize):
for id in self.limbs_names:
......@@ -179,4 +184,22 @@ class Robot (Parent):
self.addLimb(id,self.dict_limb_rootJoint[id],eff,self.dict_offset[eff].translation.T.tolist()[0],self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,heuristic,octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_com_constraints.obj",kinematicConstraintsMin=self.minDist)
if analysis :
self.runLimbSampleAnalysis(id, analysis, True)
def setConstrainedJointsBounds(self):
self.setJointBounds('leg_left_1_joint',[-0.34,1.4])
self.setJointBounds('leg_left_6_joint',[-0.25,0.25])
self.setJointBounds('leg_left_2_joint',[-0.25,0.25])
self.setJointBounds('leg_right_1_joint',[-1.4,0.34])
self.setJointBounds('leg_right_6_joint',[-0.25,0.25])
self.setJointBounds('leg_right_2_joint',[-0.25,0.25])
def resetJointsBounds(self):
self.setJointBounds('leg_left_1_joint',self.joint1L_bounds_prev)
self.setJointBounds('leg_left_6_joint',self.joint6L_bounds_prev)
self.setJointBounds('leg_left_2_joint',self.joint2L_bounds_prev)
self.setJointBounds('leg_right_1_joint',self.joint1R_bounds_prev)
self.setJointBounds('leg_right_6_joint',self.joint6R_bounds_prev)
self.setJointBounds('leg_right_2_joint',self.joint2R_bounds_prev)
src/hpp/corbaserver/rbprm/talos_fixedUpper/__init__.py 0 → 100644
View file @ 820d178c
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author : Pierre Fernbach
from robot import Robot
src/hpp/corbaserver/rbprm/talos_fixedUpper/robot.py 0 → 100644
View file @ 820d178c
#!/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 = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced_fixedUpper_safeFeet"
srdfSuffix = ""
## Information about the names of thes joints defining the limbs of the robot
rLegId = 'talos_rleg_rom'
rleg = 'leg_right_1_joint'
rfoot = 'leg_right_6_joint'
lLegId = 'talos_lleg_rom'
lleg = 'leg_left_1_joint'
lfoot = 'leg_left_6_joint'
referenceConfig = [
0.0, 0.0, 1.01927, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
]
referenceConfig_legsApart =[
0.0, 0.0, 1.01927, 0.0 , 0.0, 0.0, 1., #Free flyer
0.0, 0.06, -0.411354, 0.859395, -0.448041, -0.061708, #Left Leg
0.0, -0.06, -0.411354, 0.859395, -0.448041, 0.058292, #Right Leg
]
referenceConfig_armsFront =[
0.0, 0.0, 1.01927, 0. , 0.0, 0.0, 1., #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
]
referenceConfig_legsSide=[
0.0, 0.0, 0.9832773, 1, 0.0, 0.0, 0.0, #Free flyer
1.57, 0.0, -0.611354, 1.059395, -0.448041,-0.001708, #Left Leg
-1.57, 0.0, -0.611354, 1.059395, -0.448041, -0.001708, #Right Leg
]
postureWeights = [0,0,0,0,0,0,# freeflyer
20. , 100. , 0. , 0.1, 0. , 1., #lleg
20. , 100. , 0. , 0.1, 0. , 1., #rleg
]
postureWeights_straff = [0,0,0,0,0,0,# freeflyer
100. , 1. , 10. , 10, 1. , 0., #lleg
100. , 1. , 10. , 10, 1. , 0., #rleg
]
# informations required to generate the limbs databases :
nbSamples = 50000
octreeSize = 0.01
cType = "_6_DOF"
rLegOffset = [0., -0.00018, -0.102]
#rLegOffset[2] += 0.006
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
lLegOffset = [0., -0.00018, -0.102]
#lLegOffset[2] += 0.006
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
kinematicConstraintsPath="package://talos-rbprm/com_inequalities/"
rLegKinematicConstraints=kinematicConstraintsPath+rleg+"_com_constraints.obj"
lLegKinematicConstraints=kinematicConstraintsPath+lleg+"_com_constraints.obj"
minDist=0.4
# data used by scripts :
limbs_names = [rLegId,lLegId]
dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg}
dict_limb_joint = {rLegId:rfoot, lLegId:lfoot}
dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1]}
FOOT_SAFETY_SIZE = 0.03
# size of the contact surface (x,y)
dict_size={rfoot:[0.2 , 0.13], lfoot:[0.2 , 0.13]}
# various offset used by scripts :
MRsole_offset = SE3.Identity()
MRsole_offset.translation = np.matrix(rLegOffset).T
MLsole_offset = SE3.Identity()
MLsole_offset.translation = np.matrix(lLegOffset).T
dict_offset = {rfoot:MRsole_offset, lfoot:MLsole_offset}
dict_normal = {rfoot:rLegNormal, lfoot:lLegNormal}
# display transform :
#MRsole_display = MRsole_offset.copy()
#MLsole_display = MLsole_offset.copy()
MRsole_display = SE3.Identity()
MLsole_display = SE3.Identity()
dict_display_offset = {rfoot:MRsole_display, lfoot:MLsole_display}
kneeIds = {"Left":10,"Right":16}
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
self.joint1L_bounds_prev=self.getJointBounds('leg_left_1_joint')
self.joint6L_bounds_prev=self.getJointBounds('leg_left_6_joint')
self.joint2L_bounds_prev=self.getJointBounds('leg_left_2_joint')
self.joint1R_bounds_prev=self.getJointBounds('leg_right_1_joint')
self.joint6R_bounds_prev=self.getJointBounds('leg_right_6_joint')
self.joint2R_bounds_prev=self.getJointBounds('leg_right_2_joint')
def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize):
for id in self.limbs_names:
eff = self.dict_limb_joint[id]
self.addLimb(id,self.dict_limb_rootJoint[id],eff,self.dict_offset[eff].translation.T.tolist()[0],self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,heuristic,octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_com_constraints.obj",kinematicConstraintsMin=self.minDist)
if analysis :
self.runLimbSampleAnalysis(id, analysis, True)
def setConstrainedJointsBounds(self):
self.setJointBounds('leg_left_1_joint',[-0.34,1.4])
self.setJointBounds('leg_left_6_joint',[-0.25,0.25])
self.setJointBounds('leg_left_2_joint',[-0.25,0.25])
self.setJointBounds('leg_right_1_joint',[-1.4,0.34])
self.setJointBounds('leg_right_6_joint',[-0.25,0.25])
self.setJointBounds('leg_right_2_joint',[-0.25,0.25])
def resetJointsBounds(self):
self.setJointBounds('leg_left_1_joint',self.joint1L_bounds_prev)
self.setJointBounds('leg_left_6_joint',self.joint6L_bounds_prev)
self.setJointBounds('leg_left_2_joint',self.joint2L_bounds_prev)
self.setJointBounds('leg_right_1_joint',self.joint1R_bounds_prev)
self.setJointBounds('leg_right_6_joint',self.joint6R_bounds_prev)
self.setJointBounds('leg_right_2_joint',self.joint2R_bounds_prev)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment