diff --git a/anymal_rbprm/anymal.py b/anymal_rbprm/anymal.py
index 75744190a7bd377ab6dd26030495908baea7218a..1a25355c1520a67cf94b08875393ba3dbb7c6138 100644
--- a/anymal_rbprm/anymal.py
+++ b/anymal_rbprm/anymal.py
@@ -17,15 +17,17 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
-from pinocchio import SE3, Quaternion
+from pinocchio import SE3
import numpy as np
from pathlib import Path
+
def prefix(module):
"""$prefix/lib/pythonX.Y/site-packages/$module/__init__.py: extract prefix from module"""
return Path(module.__file__).parent.parent.parent.parent.parent
-class Robot (Parent):
+
+class Robot(Parent):
##
# Information to retrieve urdf and srdf files.
name = "anymal"
@@ -34,10 +36,10 @@ class Robot (Parent):
rootJointType = "freeflyer"
urdfName = "anymal"
urdfSuffix = ""
- #urdfSuffix="_ORI"
+ # urdfSuffix="_ORI"
srdfSuffix = ""
- ## Information about the names of thes joints defining the limbs of the robot
+ # Information about the names of thes joints defining the limbs of the robot
rLegId = 'RFleg'
rleg = 'RF_HAA'
rfoot = 'RF_ADAPTER_TO_FOOT'
@@ -51,14 +53,27 @@ class Robot (Parent):
rarm = 'RH_HAA'
rhand = 'RH_ADAPTER_TO_FOOT'
-
- referenceConfig_asymetric =[0.,0.,0.461, 0.,0.,0.,1., # FF
- 0.0, 0.611, -1.0452,
- 0.0, -0.853, 1.0847,
- -0.0, 0.74, -1.08,
- -0.0, -0.74, 1.08,
- ]
-
+ referenceConfig_asymetric = [
+ 0.,
+ 0.,
+ 0.461,
+ 0.,
+ 0.,
+ 0.,
+ 1., # FF
+ 0.0,
+ 0.611,
+ -1.0452,
+ 0.0,
+ -0.853,
+ 1.0847,
+ -0.0,
+ 0.74,
+ -1.08,
+ -0.0,
+ -0.74,
+ 1.08,
+ ]
"""
referenceConfig=[0,0,0.448,0,0,0,1,
0.079,0.78,-1.1,
@@ -66,7 +81,7 @@ class Robot (Parent):
-0.079,0.78,-1.1,
-0.079,-0.78,1.1
]
- """
+ """
"""
referenceConfig=[0,0,0.448,0,0,0,1,
0.095,0.76,-1.074,
@@ -75,69 +90,90 @@ class Robot (Parent):
-0.095,-0.76,1.074
]
"""
- referenceConfig=[0,0,0.47,0,0,0,1,
- -0.12,0.724,-1.082,
- -0.12,-0.724,1.082,
- 0.12,0.724,-1.082,
- 0.12,-0.724,1.082
- ]
- postureWeights=[0,0,0,0,0,0, #FF
- 100.,1.,20.,
- 100.,1.,20.,
- 100.,1.,20.,
- 100.,1.,20.,]
+ referenceConfig = [
+ 0, 0, 0.47, 0, 0, 0, 1, -0.12, 0.724, -1.082, -0.12, -0.724, 1.082, 0.12, 0.724, -1.082, 0.12, -0.724, 1.082
+ ]
+ postureWeights = [
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0, # FF
+ 100.,
+ 1.,
+ 20.,
+ 100.,
+ 1.,
+ 20.,
+ 100.,
+ 1.,
+ 20.,
+ 100.,
+ 1.,
+ 20.,
+ ]
DEFAULT_COM_HEIGHT = 0.445
- # informations required to generate the limbs databases the limbs :
+ # informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.002
cType = "_3_DOF"
- offset = [0.,0.,-0.005] # was 0.005
+ offset = [0., 0., -0.005] # was 0.005
rLegLimbOffset = [0.373, -0.264, -0.448]
- lLegLimbOffset = [0.373, 0.264,-0.448]
+ lLegLimbOffset = [0.373, 0.264, -0.448]
rArmLimbOffset = [-0.373, -0.264, -0.448]
lArmLimbOffset = [-0.373, 0.264, -0.448]
- normal = [0,0,1]
- legx = 0.02; legy = 0.02
+ normal = [0, 0, 1]
+ legx = 0.02
+ legy = 0.02
import anymal_rbprm
- kinematic_constraints_path = str(prefix(anymal_rbprm) / "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_")
- relative_feet_constraints_path = str(prefix(anymal_rbprm) / "share/anymal-rbprm/relative_effector_positions/anymal_")
-
+ kinematic_constraints_path = str(
+ prefix(anymal_rbprm) / "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_")
+ relative_feet_constraints_path = str(
+ prefix(anymal_rbprm) / "share/anymal-rbprm/relative_effector_positions/anymal_")
+
minDist = 0.2
-
- dict_ref_effector_from_root = {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
+ dict_ref_effector_from_root = {
+ rLegId: rLegLimbOffset,
+ lLegId: lLegLimbOffset,
+ rArmId: rArmLimbOffset,
+ lArmId: lArmLimbOffset
+ }
# data used by scripts :,,,
- #limbs_names = [rArmId,lLegId,lArmId,rLegId] # reverse default order to try to remove contacts at the beginning of the contact plan
- #limbs_names = [lLegId,rArmId,rLegId,lArmId] # default order to try to remove contacts at the beginning of the contact plan
- limbs_names = [rArmId,rLegId,lArmId,lLegId]
- dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
- 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]}
+ # limbs_names = [rArmId,lLegId,lArmId,rLegId]
+ # reverse default order to try to remove contacts at the beginning of the contact plan
+ # limbs_names = [lLegId,rArmId,rLegId,lArmId]
+ # default order to try to remove contacts at the beginning of the contact plan
+ limbs_names = [rArmId, rLegId, lArmId, lLegId]
+ dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
+ 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.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
- #dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
- #various offset used by scripts
+ dict_size = {rfoot: [0.01, 0.01], lfoot: [0.01, 0.01], rhand: [0.01, 0.01], lhand: [0.01, 0.01]}
+ # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
+ # 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}
- dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
- dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
+ dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
+ dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
+ dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# 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}
+ dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
- kneeIds = {"LF":9,"LH":12,"RF":15,"RH":18}
+ kneeIds = {"LF": 9, "LH": 12, "RF": 15, "RH": 18}
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
@@ -160,108 +196,109 @@ class Robot (Parent):
self.RH_HFE_bounds = self.getJointBounds('RH_HFE')
self.RH_KFE_bounds = self.getJointBounds('RH_KFE')
- def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize,disableEffectorCollision = False):
- if isinstance(heuristic,str):#only one heuristic name given assign it to all the limbs
+ def loadAllLimbs(self,
+ heuristic,
+ analysis=None,
+ nbSamples=nbSamples,
+ octreeSize=octreeSize,
+ disableEffectorCollision=False):
+ if isinstance(heuristic, str): # only one heuristic name given assign it to all the limbs
dict_heuristic = {}
for id in self.limbs_names:
- dict_heuristic.update({id:heuristic})
- elif isinstance(heuristic,dict):
- dict_heuristic=heuristic
- else :
+ dict_heuristic.update({id: heuristic})
+ elif isinstance(heuristic, dict):
+ dict_heuristic = heuristic
+ else:
raise Exception("heuristic should be either a string or a map limbId:string")
- #dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04", self.lArmId:"fixedStep04"}
+ # dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04",
+ # self.lArmId:"fixedStep04"}
for id in self.limbs_names:
- print("add limb : ",id)
+ print("add limb : ", id)
eff = self.dict_limb_joint[id]
- print("effector name = ",eff)
+ print("effector name = ", eff)
self.addLimb(id,
self.dict_limb_rootJoint[id],
eff,
self.dict_offset[eff].translation.tolist(),
self.dict_normal[eff],
- self.dict_size[eff][0]/2.,
- self.dict_size[eff][1]/2.,
+ self.dict_size[eff][0] / 2.,
+ self.dict_size[eff][1] / 2.,
nbSamples,
dict_heuristic[id],
octreeSize,
self.cType,
- disableEffectorCollision = disableEffectorCollision,
- kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_06_com_constraints.obj",
+ disableEffectorCollision=disableEffectorCollision,
+ kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
+ "_06_com_constraints.obj",
limbOffset=self.dict_limb_offset[id],
kinematicConstraintsMin=self.minDist)
- if analysis :
+ if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
def setSlightlyConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA',[-1.,1.])
- self.setJointBounds('LF_HFE',[-0.25,2.35])
- self.setJointBounds('LF_KFE',[-2.35,0.])
+ self.setJointBounds('LF_HAA', [-1., 1.])
+ self.setJointBounds('LF_HFE', [-0.25, 2.35])
+ self.setJointBounds('LF_KFE', [-2.35, 0.])
- self.setJointBounds('RF_HAA',[-1.,1.])
- self.setJointBounds('RF_HFE',[-0.4,2.35])
- self.setJointBounds('RF_KFE',[-2.35,0.])
+ self.setJointBounds('RF_HAA', [-1., 1.])
+ self.setJointBounds('RF_HFE', [-0.4, 2.35])
+ self.setJointBounds('RF_KFE', [-2.35, 0.])
- self.setJointBounds('LH_HAA',[-1.,1.])
- self.setJointBounds('LH_HFE',[-2.35,0.4])
- self.setJointBounds('LH_KFE',[0.,2.35])
-
- self.setJointBounds('RH_HAA',[-1.,1.])
- self.setJointBounds('RH_HFE',[-2.35,0.25])
- self.setJointBounds('RH_KFE',[0.,2.35])
+ self.setJointBounds('LH_HAA', [-1., 1.])
+ self.setJointBounds('LH_HFE', [-2.35, 0.4])
+ self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds('RH_HAA', [-1., 1.])
+ self.setJointBounds('RH_HFE', [-2.35, 0.25])
+ self.setJointBounds('RH_KFE', [0., 2.35])
def setConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA',[-0.6,0.6])
- self.setJointBounds('LF_HFE',[0.25,1.])
- self.setJointBounds('LF_KFE',[-2.35,0.])
-
- self.setJointBounds('RF_HAA',[-0.6,0.6])
- self.setJointBounds('RF_HFE',[0.25,1.])
- self.setJointBounds('RF_KFE',[-2.35,0.])
+ self.setJointBounds('LF_HAA', [-0.6, 0.6])
+ self.setJointBounds('LF_HFE', [0.25, 1.])
+ self.setJointBounds('LF_KFE', [-2.35, 0.])
- self.setJointBounds('LH_HAA',[-0.6,0.6])
- self.setJointBounds('LH_HFE',[-1.05,-0.45])
- self.setJointBounds('LH_KFE',[0.,2.35])
+ self.setJointBounds('RF_HAA', [-0.6, 0.6])
+ self.setJointBounds('RF_HFE', [0.25, 1.])
+ self.setJointBounds('RF_KFE', [-2.35, 0.])
- self.setJointBounds('RH_HAA',[-0.6,0.6])
- self.setJointBounds('RH_HFE',[-1.05,-0.45])
- self.setJointBounds('RH_KFE',[0.,2.35])
+ self.setJointBounds('LH_HAA', [-0.6, 0.6])
+ self.setJointBounds('LH_HFE', [-1.05, -0.45])
+ self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds('RH_HAA', [-0.6, 0.6])
+ self.setJointBounds('RH_HFE', [-1.05, -0.45])
+ self.setJointBounds('RH_KFE', [0., 2.35])
def setVeryConstrainedJointsBounds(self):
- self.setJointBounds('LF_HAA',[-0.35,0.05])
- self.setJointBounds('LF_HFE',[0.3,0.95])
- self.setJointBounds('LF_KFE',[-2.35,0.])
-
- self.setJointBounds('RF_HAA',[-0.05,0.35])
- self.setJointBounds('RF_HFE',[0.3,0.95])
- self.setJointBounds('RF_KFE',[-2.35,0.])
+ self.setJointBounds('LF_HAA', [-0.35, 0.05])
+ self.setJointBounds('LF_HFE', [0.3, 0.95])
+ self.setJointBounds('LF_KFE', [-2.35, 0.])
- self.setJointBounds('LH_HAA',[-0.35,0.05])
- self.setJointBounds('LH_HFE',[-1.,-0.5])
- self.setJointBounds('LH_KFE',[0.,2.35])
+ self.setJointBounds('RF_HAA', [-0.05, 0.35])
+ self.setJointBounds('RF_HFE', [0.3, 0.95])
+ self.setJointBounds('RF_KFE', [-2.35, 0.])
- self.setJointBounds('RH_HAA',[-0.05,0.35])
- self.setJointBounds('RH_HFE',[-1.,-0.5])
- self.setJointBounds('RH_KFE',[0.,2.35])
+ self.setJointBounds('LH_HAA', [-0.35, 0.05])
+ self.setJointBounds('LH_HFE', [-1., -0.5])
+ self.setJointBounds('LH_KFE', [0., 2.35])
+ self.setJointBounds('RH_HAA', [-0.05, 0.35])
+ self.setJointBounds('RH_HFE', [-1., -0.5])
+ self.setJointBounds('RH_KFE', [0., 2.35])
def resetJointsBounds(self):
- self.setJointBounds('LF_HAA',self.LF_HAA_bounds)
- self.setJointBounds('LF_HFE',self.LF_HFE_bounds)
- self.setJointBounds('LF_KFE',self.LF_KFE_bounds)
-
- self.setJointBounds('RF_HAA',self.RF_HAA_bounds)
- self.setJointBounds('RF_HFE',self.RF_HFE_bounds)
- self.setJointBounds('RF_KFE',self.RF_KFE_bounds)
-
- self.setJointBounds('LH_HAA',self.LH_HAA_bounds)
- self.setJointBounds('LH_HFE',self.LH_HFE_bounds)
- self.setJointBounds('LH_KFE',self.LH_KFE_bounds)
+ self.setJointBounds('LF_HAA', self.LF_HAA_bounds)
+ self.setJointBounds('LF_HFE', self.LF_HFE_bounds)
+ self.setJointBounds('LF_KFE', self.LF_KFE_bounds)
- self.setJointBounds('RH_HAA',self.RH_HAA_bounds)
- self.setJointBounds('RH_HFE',self.RH_HFE_bounds)
- self.setJointBounds('RH_KFE',self.RH_KFE_bounds)
+ self.setJointBounds('RF_HAA', self.RF_HAA_bounds)
+ self.setJointBounds('RF_HFE', self.RF_HFE_bounds)
+ self.setJointBounds('RF_KFE', self.RF_KFE_bounds)
+ self.setJointBounds('LH_HAA', self.LH_HAA_bounds)
+ self.setJointBounds('LH_HFE', self.LH_HFE_bounds)
+ self.setJointBounds('LH_KFE', self.LH_KFE_bounds)
-
+ self.setJointBounds('RH_HAA', self.RH_HAA_bounds)
+ self.setJointBounds('RH_HFE', self.RH_HFE_bounds)
+ self.setJointBounds('RH_KFE', self.RH_KFE_bounds)
diff --git a/anymal_rbprm/anymal_abstract.py b/anymal_rbprm/anymal_abstract.py
index b8d40bb8ae28c672b5fedaf669eb9bf29d49015d..b810ce9cbccd37a54e3b2f42e50512340351b5d7 100644
--- a/anymal_rbprm/anymal_abstract.py
+++ b/anymal_rbprm/anymal_abstract.py
@@ -18,8 +18,8 @@
from hpp.corbaserver.rbprm.rbprmbuilder import Builder as Parent
-class Robot (Parent):
- ##
+
+class Robot(Parent):
# Information to retrieve urdf and srdf files.
rootJointType = 'freeflyer'
packageName = 'anymal-rbprm'
@@ -27,7 +27,7 @@ class Robot (Parent):
# URDF file describing the trunk of the robot HyQ
urdfName = 'anymal_trunk'
# URDF files describing the reachable workspace of each limb of HyQ
- urdfNameRom = ['anymal_RFleg_rom','anymal_LHleg_rom','anymal_LFleg_rom','anymal_RHleg_rom']
+ urdfNameRom = ['anymal_RFleg_rom', 'anymal_LHleg_rom', 'anymal_LFleg_rom', 'anymal_RHleg_rom']
urdfSuffix = ""
srdfSuffix = ""
name = urdfName
@@ -35,32 +35,28 @@ class Robot (Parent):
ref_height = 0.465
# TODO
-
-
+
rLegId = 'anymal_RFleg_rom'
lLegId = 'anymal_LFleg_rom'
rArmId = 'anymal_RHleg_rom'
lArmId = 'anymal_LHleg_rom'
-
- ref_EE_lLeg =[0.373, 0.264, -0.448]
+
+ ref_EE_lLeg = [0.373, 0.264, -0.448]
ref_EE_rLeg = [0.373, -0.264, -0.448]
ref_EE_lArm = [-0.373, 0.264, -0.448]
ref_EE_rArm = [-0.373, -0.264, -0.448]
- #ref_EE_lLeg = [0.3, 0.165 , -0.44]
- #ref_EE_rLeg = [0.3, -0.165 , -0.44]
- #ref_EE_lArm = [-0.3, 0.165 , -0.44]
- #ref_EE_rArm = [-0.3, -0.165 , -0.44]
-
- dict_ref_effector_from_root = {rLegId:ref_EE_rLeg,
- lLegId:ref_EE_lLeg,
- rArmId:ref_EE_rArm,
- lArmId:ref_EE_lArm}
+ # ref_EE_lLeg = [0.3, 0.165 , -0.44]
+ # ref_EE_rLeg = [0.3, -0.165 , -0.44]
+ # ref_EE_lArm = [-0.3, 0.165 , -0.44]
+ # ref_EE_rArm = [-0.3, -0.165 , -0.44]
+
+ dict_ref_effector_from_root = {rLegId: ref_EE_rLeg, lLegId: ref_EE_lLeg, rArmId: ref_EE_rArm, lArmId: ref_EE_lArm}
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
Parent.__init__(self, self.name, self.rootJointType, load, client, None, clientRbprm)
- self.setReferenceEndEffector('anymal_LFleg_rom',self.ref_EE_lLeg)
- self.setReferenceEndEffector('anymal_RFleg_rom',self.ref_EE_rLeg)
- self.setReferenceEndEffector('anymal_LHleg_rom',self.ref_EE_lArm)
- self.setReferenceEndEffector('anymal_RHleg_rom',self.ref_EE_rArm)
+ self.setReferenceEndEffector('anymal_LFleg_rom', self.ref_EE_lLeg)
+ self.setReferenceEndEffector('anymal_RFleg_rom', self.ref_EE_rLeg)
+ self.setReferenceEndEffector('anymal_LHleg_rom', self.ref_EE_lArm)
+ self.setReferenceEndEffector('anymal_RHleg_rom', self.ref_EE_rArm)
diff --git a/anymal_rbprm/anymal_contact6D.py b/anymal_rbprm/anymal_contact6D.py
index 597690799f1a564852a6cca922a91547818130c4..9c8112ec5ba162b4f8dd2a024798fb9f7338f880 100644
--- a/anymal_rbprm/anymal_contact6D.py
+++ b/anymal_rbprm/anymal_contact6D.py
@@ -17,11 +17,11 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
-from pinocchio import SE3, Quaternion
+from pinocchio import SE3
import numpy as np
-class Robot (Parent):
- ##
+
+class Robot(Parent):
# Information to retrieve urdf and srdf files.
packageName = "anymal_description"
@@ -31,7 +31,7 @@ class Robot (Parent):
urdfSuffix = "_boxFeet"
srdfSuffix = ""
- ## Information about the names of thes joints defining the limbs of the robot
+ # Information about the names of thes joints defining the limbs of the robot
rLegId = 'RFleg'
rleg = 'RF_HAA'
rfoot = 'RF_CONTACT_3'
@@ -45,81 +45,125 @@ class Robot (Parent):
rarm = 'RH_HAA'
rhand = 'RH_CONTACT_3'
+ referenceConfig = [
+ 0.,
+ 0.,
+ 0.444,
+ 0.,
+ 0.,
+ 0.,
+ 1., # FF
+ 0.04,
+ 0.74,
+ -1.08,
+ 0.34,
+ -0.04,
+ 0.,
+ 0.04,
+ -0.74,
+ 1.08,
+ -0.34,
+ -0.04,
+ 0.,
+ -0.04,
+ 0.74,
+ -1.08,
+ 0.34,
+ 0.04,
+ 0.,
+ -0.04,
+ -0.74,
+ 1.08,
+ -0.34,
+ 0.04,
+ 0.
+ ]
- referenceConfig =[0.,0.,0.444, 0.,0.,0.,1., # FF
- 0.04, 0.74, -1.08,0.34,-0.04,0.,
- 0.04, -0.74, 1.08,-0.34,-0.04,0.,
- -0.04, 0.74, -1.08,0.34,0.04, 0.,
- -0.04, -0.74, 1.08,-0.34,0.04,0.
- ]
-
- reference_weights=[100.,1.,1.,0.,0.,0.]
+ reference_weights = [100., 1., 1., 0., 0., 0.]
- # informations required to generate the limbs databases the limbs :
+ # informations required to generate the limbs databases the limbs :
nbSamples = 50000
octreeSize = 0.01
cType = "_6_DOF"
- offset = [0.,0.,0.]
+ offset = [0., 0., 0.]
rLegLimbOffset = [0.373, 0.264, 0.]
- lLegLimbOffset = [0.373, -0.264,0.]
+ lLegLimbOffset = [0.373, -0.264, 0.]
rArmLimbOffset = [-0.373, 0.264, 0.]
lArmLimbOffset = [-0.373, -0.264, 0.]
- normal = [0,0,1]
- legx = 0.02; legy = 0.02
- kinematicConstraintsPath="package://anymal-rbprm/com_inequalities/"
+ normal = [0, 0, 1]
+ legx = 0.02
+ legy = 0.02
+ kinematicConstraintsPath = "package://anymal-rbprm/com_inequalities/"
- minDist = 0.2
+ minDist = 0.2
# data used by scripts :,,,
- #limbs_names = [rArmId,lLegId,lArmId,rLegId] # reverse default order to try to remove contacts at the beginning of the contact plan
- #limbs_names = [lLegId,rArmId,rLegId,lArmId] # default order to try to remove contacts at the beginning of the contact plan
- limbs_names = [rArmId,lArmId,lLegId,rLegId]
- dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
- 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]}
+ # limbs_names = [rArmId,lLegId,lArmId,rLegId]
+ # reverse default order to try to remove contacts at the beginning of the contact plan
+ # limbs_names = [lLegId,rArmId,rLegId,lArmId]
+ # default order to try to remove contacts at the beginning of the contact plan
+ limbs_names = [rArmId, lArmId, lLegId, rLegId]
+ dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
+ 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.031 , 0.031], lfoot:[0.031 , 0.031],rhand:[0.031 , 0.031],lhand:[0.031 , 0.031]}
- #dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
- #various offset used by scripts
+ dict_size = {rfoot: [0.031, 0.031], lfoot: [0.031, 0.031], rhand: [0.031, 0.031], lhand: [0.031, 0.031]}
+ # dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
+ # 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}
- dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
- dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
+ dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
+ dict_limb_offset = {rLegId: rLegLimbOffset, lLegId: lLegLimbOffset, rArmId: rArmLimbOffset, lArmId: lArmLimbOffset}
+ dict_normal = {rfoot: normal, lfoot: normal, rhand: normal, lhand: normal}
# 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}
+ 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)
+ 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.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName,
+ self.urdfSuffix, self.srdfSuffix)
+ if name is not None:
self.name = name
- def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize):
- if isinstance(heuristic,basestring):#only one heuristic name given assign it to all the limbs
+ def loadAllLimbs(self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize):
+ if isinstance(heuristic, str): # only one heuristic name given assign it to all the limbs
dict_heuristic = {}
for id in self.limbs_names:
- dict_heuristic.update({id:heuristic})
- elif isinstance(heuristic,dict):
- dict_heuristic=heuristic
- else :
+ dict_heuristic.update({id: heuristic})
+ elif isinstance(heuristic, dict):
+ dict_heuristic = heuristic
+ else:
raise Exception("heuristic should be either a string or a map limbId:string")
- #dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04", self.lArmId:"fixedStep04"}
+ # dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04",
+ # self.lArmId:"fixedStep04"}
for id in self.limbs_names:
- print("add limb : ",id)
+ print("add limb : ", id)
eff = self.dict_limb_joint[id]
- print("effector name = ",eff)
- self.addLimb(id,self.dict_limb_rootJoint[id],eff,self.dict_offset[eff].translation.tolist(),self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,dict_heuristic[id],octreeSize,self.cType,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"06_com_constraints.obj",limbOffset=self.dict_limb_offset[id],kinematicConstraintsMin=self.minDist)
- if analysis :
+ print("effector name = ", eff)
+ self.addLimb(id,
+ self.dict_limb_rootJoint[id],
+ eff,
+ self.dict_offset[eff].translation.tolist(),
+ self.dict_normal[eff],
+ self.dict_size[eff][0] / 2.,
+ self.dict_size[eff][1] / 2.,
+ nbSamples,
+ dict_heuristic[id],
+ octreeSize,
+ self.cType,
+ kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
+ "06_com_constraints.obj",
+ limbOffset=self.dict_limb_offset[id],
+ kinematicConstraintsMin=self.minDist)
+ if analysis:
self.runLimbSampleAnalysis(id, analysis, True)
-
diff --git a/script/relative_foot_positions/constants_and_tools.py b/script/relative_foot_positions/constants_and_tools.py
index 65c4f26e0be6376a16f2dbde13bdf71b7d496eca..890906ea707b1b3f4db6d6bacfc8d6e010ba6b08 100644
--- a/script/relative_foot_positions/constants_and_tools.py
+++ b/script/relative_foot_positions/constants_and_tools.py
@@ -1,153 +1,154 @@
-import numpy as np
-#~ from hpp.corbaserver.rbprm.hrp2 import Robot as rob
-#~ from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
-#~ from hpp_centroidal_dynamics import *
-#~ from hpp_spline import *e
-from numpy import array, asmatrix, matrix, zeros, ones
-from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
-from numpy.linalg import norm
+# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
+# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
+# from hpp_centroidal_dynamics import *
+# from hpp_spline import *e
+from numpy import array, matrix, zeros, ones, vstack, hstack, identity, concatenate
import numpy as np
from scipy.spatial import ConvexHull
-#~ from hpp_bezier_com_traj import *
-#~ from qp import solve_lp
-
-#~ import eigenpy
-#~ import cdd
-#~ from curves import bezier3
-from random import random as rd
-from random import randint as rdi
-from numpy import squeeze, asarray
+# from hpp_bezier_com_traj import *
+# from qp import solve_lp
+# import eigenpy
+import cdd
+# from curves import bezier3
Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
-z = array([0.,0.,1.])
-zero3 = zeros(3)
+z = array([0., 0., 1.])
+zero3 = zeros(3)
-def generators(A,b, Aeq = None, beq = None):
- m = np.hstack([b,-A])
- matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.INEQUALITY
-
+def generators(A, b, Aeq=None, beq=None):
+ m = np.hstack([b, -A])
+ matcdd = cdd.Matrix(m)
+ matcdd.rep_type = cdd.RepType.INEQUALITY
+
if Aeq is not None:
- meq = np.hstack([beq,-Aeq])
+ meq = np.hstack([beq, -Aeq])
matcdd.extend(meq.tolist(), True)
-
+
H = cdd.Polyhedron(matcdd)
g = H.get_generators()
-
+
return [array(g[el][1:]) for el in range(g.row_size)], H
-
+
+
def filter(pts):
hull = ConvexHull(pts, qhull_options='Q12')
return [pts[i] for i in hull.vertices.tolist()]
-
-def ineq(pts, canonicalize = False):
+
+
+def ineq(pts, canonicalize=False):
apts = array(pts)
- m = np.hstack([ones((apts.shape[0],1)),apts])
- matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.GENERATOR
+ m = np.hstack([ones((apts.shape[0], 1)), apts])
+ matcdd = cdd.Matrix(m)
+ matcdd.rep_type = cdd.RepType.GENERATOR
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
if canonicalize:
bmA.canonicalize()
- Ares = zeros((bmA.row_size,bmA.col_size-1))
- bres = zeros(bmA.row_size )
+ Ares = zeros((bmA.row_size, bmA.col_size - 1))
+ bres = zeros(bmA.row_size)
for i in range(bmA.row_size):
- l = array(bmA[i])
- Ares[i,:] = -l[1:]
- bres[i] = l[0]
+ j = array(bmA[i])
+ Ares[i, :] = -j[1:]
+ bres[i] = j[0]
return Ares, bres
-
+
+
def ineqQHull(hull):
- A = hull.equations[:,:-1]
- b = -hull.equations[:,-1]
- return A,b
-
-
-def canon(A,b):
- m = np.hstack([b,-A])
- matcdd = cdd.Matrix(m); matcdd.rep_type = 1
+ A = hull.equations[:, :-1]
+ b = -hull.equations[:, -1]
+ return A, b
+
+
+def canon(A, b):
+ m = np.hstack([b, -A])
+ matcdd = cdd.Matrix(m)
+ matcdd.rep_type = 1
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
- #~ bmA.canonicalize()
- Ares = zeros((bmA.row_size,bmA.col_size-1))
- bres = zeros((bmA.row_size,1 ))
+ # bmA.canonicalize()
+ Ares = zeros((bmA.row_size, bmA.col_size - 1))
+ bres = zeros((bmA.row_size, 1))
for i in range(bmA.row_size):
- #~ print "line ", array(bmA[i])
- #~ print "A ", A[i][:]
- #~ print "b ", b[i]
- l = array(bmA[i])
- Ares[i,:] = -l[1:]
- bres[i] = l[0]
- #~ print "Ares ",Ares[i,:]
- #~ print "bres ",bres[i]
+ # print "line ", array(bmA[i])
+ # print "A ", A[i][:]
+ # print "b ", b[i]
+ j = array(bmA[i])
+ Ares[i, :] = -j[1:]
+ bres[i] = j[0]
+ # print "Ares ",Ares[i,:]
+ # print "bres ",bres[i]
return Ares, bres
-def genPolytope(A,b):
- pts, H = generators(A,b)
+
+def genPolytope(A, b):
+ pts, H = generators(A, b)
apts = array(pts)
if len(apts) > 0:
hull = ConvexHull(apts)
return hull, pts, apts, H
return None, None, None, None
-
-def convex_hull_ineq(pts):
+
+
+def convex_hull_ineq(pts, cData=None, ineqFromCdata=None, gX=None, g=None, w=None):
return None
-
-
+
m = cData.contactPhase_.getMass()
- #~ #get 6D polytope
+ # #get 6D polytope
(H, h) = ineqFromCdata(cData)
- #project to the space where aceleration is 0
- D = zeros((6,3))
- D[3:,:] = m * gX
-
- d = zeros((6,))
+ # project to the space where aceleration is 0
+ D = zeros((6, 3))
+ D[3:, :] = m * gX
+
+ d = zeros((6, ))
d[:3] = -m * g
-
- A = H.dot(D)
- b = h.reshape((-1,)) - H.dot(d)
- #add kinematic polytope
- (K,k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1,))
-
+
+ A = H.dot(D)
+ b = h.reshape((-1, )) - H.dot(d)
+ # add kinematic polytope
+ (K, k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1, ))
+
resA = vstack([A, K])
- resb = concatenate([b, k]).reshape((-1,1))
-
- #DEBUG
- allpts = generators(resA,resb)[0]
+ resb = concatenate([b, k]).reshape((-1, 1))
+
+ # DEBUG
+ allpts = generators(resA, resb)[0]
error = False
for pt in allpts:
- print ("pt ", pt)
- assert (resA.dot(pt.reshape((-1,1))) - resb).max() <0.001, "antecedent point not in End polytope" + str((resA.dot(pt.reshape((-1,1))) - resb).max())
- if (H.dot(w(m,pt).reshape((-1,1))) - h).max() > 0.001:
+ print("pt ", pt)
+ assert (resA.dot(pt.reshape((-1, 1))) - resb).max() < 0.001, "antecedent point not in End polytope" + str(
+ (resA.dot(pt.reshape((-1, 1))) - resb).max())
+ if (H.dot(w(m, pt).reshape((-1, 1))) - h).max() > 0.001:
error = True
- print ("antecedent point not in End polytope" + str((H.dot(w(m,pt).reshape((-1,1))) - h).max()))
- assert not error, str (len(allpts))
-
+ print("antecedent point not in End polytope" + str((H.dot(w(m, pt).reshape((-1, 1))) - h).max()))
+ assert not error, str(len(allpts))
+
return (resA, resb)
- #~ return (A, b)
- #~ return (vstack([A, K]), None)
+ # return (A, b)
+ # return (vstack([A, K]), None)
+
def default_transform_from_pos_normal(pos, normal):
- #~ print "pos ", pos
- #~ print "normal ", normal
- f = array([0.,0.,1.])
+ # print "pos ", pos
+ # print "normal ", normal
+ f = array([0., 0., 1.])
t = array(normal)
v = np.cross(f, t)
c = np.dot(f, t)
if c > 0.99:
- rot = identity(3)
+ rot = identity(3)
else:
- u = v/norm(v)
- h = (1. - c)/(1. - c**2)
+ # u = v / norm(v)
+ h = (1. - c) / (1. - c**2)
vx, vy, vz = v
- rot =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
- [h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
- [h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
- return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] )
+ rot = array([[c + h * vx**2, h * vx * vy - vz, h * vx * vz + vy],
+ [h * vx * vy + vz, c + h * vy**2, h * vy * vz - vx],
+ [h * vx * vz - vy, h * vy * vz + vx, c + h * vz**2]])
+ return vstack([hstack([rot, pos.reshape((-1, 1))]), [0., 0., 0., 1.]])
-import os
def continuous(h, initpts):
dic = {}
@@ -157,44 +158,43 @@ def continuous(h, initpts):
dic[pt] = i
faces = []
for f in h.simplices:
- faces += [[dic[idx]+1 for idx in f ]]
+ faces += [[dic[idx] + 1 for idx in f]]
return pts, faces
-def hull_to_obj(h,pts,name):
+
+def hull_to_obj(h, pts, name):
pts, faces = continuous(h, pts)
f = open(name, "w")
- #first write points
+ # first write points
for pt in pts:
- #~ print "??"
- f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n' );
+ # print "??"
+ f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
f.write('g foo\n')
for pt in faces:
- #~ print "???"
- f.write('f ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n' );
+ # print "???"
+ f.write('f ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
f.write('g \n')
f.close()
-
-
-
-#~ function vertface2obj(v,f,name)
-#~ % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
-#~ % VERTFACE2OBJ(v,f,fname)
-#~ % v is a Nx3 matrix of vertex coordinates.
-#~ % f is a Mx3 matrix of vertex indices.
-#~ % fname is the filename to save the obj file.
-#~ fid = fopen(name,'w');
+# function vertface2obj(v,f,name)
+# % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
+# % VERTFACE2OBJ(v,f,fname)
+# % v is a Nx3 matrix of vertex coordinates.
+# % f is a Mx3 matrix of vertex indices.
+# % fname is the filename to save the obj file.
+
+# fid = fopen(name,'w');
-#~ for i=1:size(v,1)
-#~ fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
-#~ end
+# for i=1:size(v,1)
+# fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
+# end
-#~ fprintf(fid,'g foo\n');
+# fprintf(fid,'g foo\n');
-#~ for i=1:size(f,1);
-#~ fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
-#~ end
-#~ fprintf(fid,'g\n');
+# for i=1:size(f,1);
+# fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
+# end
+# fprintf(fid,'g\n');
-#~ fclose(fid);
+# fclose(fid);
diff --git a/script/relative_foot_positions/obj_to_constraints.py b/script/relative_foot_positions/obj_to_constraints.py
index f7d7997ec3e6d26d23858a0c0b56d871b12c65db..053635c230a5db84b550e51e8efec47210cd2034 100644
--- a/script/relative_foot_positions/obj_to_constraints.py
+++ b/script/relative_foot_positions/obj_to_constraints.py
@@ -1,156 +1,169 @@
-#do the loading of the obj file
+# do the loading of the obj file
import numpy as np
+from pickle import load, dump
from collections import namedtuple
+
ObjectData = namedtuple("ObjectData", "V T N F")
Inequalities = namedtuple("Inequality", "A b N V")
+
def toFloat(stringArray):
- res= np.zeros(len(stringArray))
- for i in range(0,len(stringArray)):
- res[i] = float(stringArray[i])
- return res
-
-def load_obj(filename) :
- V = [] #vertex
- T = [] #texcoords
- N = [] #normals
- F = [] #face indexies
-
- fh = open(filename)
- for line in fh :
- if line[0] == '#' : continue
-
- line = line.strip().split(' ')
- if line[0] == 'v' : #vertex
- V.append(toFloat(line[1:]))
- elif line[0] == 'vt' : #tex-coord
- T.append(line[1:])
- elif line[0] == 'vn' : #normal vector
- N.append(toFloat(line[1:]))
- elif line[0] == 'f' : #face
- face = line[1:]
- for i in range(0, len(face)) :
- face[i] = face[i].split('/')
- # OBJ indexies are 1 based not 0 based hence the -1
- # convert indexies to integer
- for j in range(0, len(face[i])):
- if j!=1:
- face[i][j] = int(face[i][j]) - 1
- F.append(face)
-
- return ObjectData(V, T, N, F)
-
-def inequality(v, n):
- #the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
- #inequality is then ax + by +cz -d <= 0
- # last var is v because we need it
- return [n[0], n[1], n[2], np.array(v).dot(np.array(n))]
-
+ res = np.zeros(len(stringArray))
+ for i in range(0, len(stringArray)):
+ res[i] = float(stringArray[i])
+ return res
+
+
+def load_obj(filename):
+ V = [] # vertex
+ T = [] # texcoords
+ N = [] # normals
+ F = [] # face indexies
+
+ fh = open(filename)
+ for line in fh:
+ if line[0] == '#':
+ continue
+
+ line = line.strip().split(' ')
+ if line[0] == 'v': # vertex
+ V.append(toFloat(line[1:]))
+ elif line[0] == 'vt': # tex-coord
+ T.append(line[1:])
+ elif line[0] == 'vn': # normal vector
+ N.append(toFloat(line[1:]))
+ elif line[0] == 'f': # face
+ face = line[1:]
+ for i in range(0, len(face)):
+ face[i] = face[i].split('/')
+ # OBJ indexies are 1 based not 0 based hence the -1
+ # convert indexies to integer
+ for j in range(0, len(face[i])):
+ if j != 1:
+ face[i][j] = int(face[i][j]) - 1
+ F.append(face)
+
+ return ObjectData(V, T, N, F)
+
+
+def inequality(v, n):
+ # the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
+ # inequality is then ax + by +cz -d <= 0
+ # last var is v because we need it
+ return [n[0], n[1], n[2], np.array(v).dot(np.array(n))]
+
+
def as_inequalities(obj):
- #for each face, find first three points and deduce plane
- #inequality is given by normal
- A= np.empty([len(obj.F), 3])
- b = np.empty(len(obj.F))
- V = np.ones([len(obj.F), 4])
- N = np.empty([len(obj.F), 3])
- for f in range(0, len(obj.F)):
- face = obj.F[f]
- v = obj.V[face[0][0]]
- # assume normals are in obj
- n = obj.N[face[0][2]]
- ineq = inequality(v,n)
- A[f,:] = ineq[0:3]
- b[f] = ineq[3]
- V[f,0:3] = v
- N[f,:] = n
- return Inequalities(A,b, N, V)
-
+ # for each face, find first three points and deduce plane
+ # inequality is given by normal
+ A = np.empty([len(obj.F), 3])
+ b = np.empty(len(obj.F))
+ V = np.ones([len(obj.F), 4])
+ N = np.empty([len(obj.F), 3])
+ for f in range(0, len(obj.F)):
+ face = obj.F[f]
+ v = obj.V[face[0][0]]
+ # assume normals are in obj
+ n = obj.N[face[0][2]]
+ ineq = inequality(v, n)
+ A[f, :] = ineq[0:3]
+ b[f] = ineq[3]
+ V[f, 0:3] = v
+ N[f, :] = n
+ return Inequalities(A, b, N, V)
+
+
def is_inside(inequalities, pt):
- return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
+ return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
+
+
+# def rotate_inequalities_q():
-#~ def rotate_inequalities_q():
# TODO this is naive, should be a way to simply update d
def rotate_inequalities(ineq, transform):
- #for each face, find first three points and deduce plane
- #inequality is given by normal
- A = np.empty([len(ineq.A), 3])
- b = np.empty(len(ineq.b))
- V = np.ones([len(ineq.V), 4])
- N = np.ones([len(ineq.N), 3])
- for i in range(0, len(b)):
- v = transform.dot(ineq.V[i,:])
- n = transform[0:3,0:3].dot(ineq.N[i,0:3])
- ine = inequality(v[0:3],n[0:3])
- A[i,:] = ine[0:3]
- b[i] = ine[3]
- V[i,:] = v
- N[i,:] = n
- return Inequalities(A,b, N, V)
-
-from pickle import dump
+ # for each face, find first three points and deduce plane
+ # inequality is given by normal
+ A = np.empty([len(ineq.A), 3])
+ b = np.empty(len(ineq.b))
+ V = np.ones([len(ineq.V), 4])
+ N = np.ones([len(ineq.N), 3])
+ for i in range(0, len(b)):
+ v = transform.dot(ineq.V[i, :])
+ n = transform[0:3, 0:3].dot(ineq.N[i, 0:3])
+ ine = inequality(v[0:3], n[0:3])
+ A[i, :] = ine[0:3]
+ b[i] = ine[3]
+ V[i, :] = v
+ N[i, :] = n
+ return Inequalities(A, b, N, V)
+
+
def ineq_to_file(ineq, filename):
- f1=open(filename, 'w+')
- res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
- dump(res, f1)
- f1.close()
-
-from pickle import load
+ f1 = open(filename, 'w+')
+ res = {'A': ineq.A, 'b': ineq.b, 'N': ineq.N, 'V': ineq.V}
+ dump(res, f1)
+ f1.close()
+
+
def ineq_from_file(filename):
- f1=open(filename, 'r')
- res = load(f1)
- return Inequalities(res['A'], res['b'],res['N'],res['V'])
-
+ f1 = open(filename, 'r')
+ res = load(f1)
+ return Inequalities(res['A'], res['b'], res['N'], res['V'])
+
+
def test_inequality():
- n = np.array([0,-1,0])
- v = np.array([0,1,1])
- if inequality(v,n) != [0,-1,0,-1]:
- print("error in test_inequality")
- else:
- print("test_inequality successful")
+ n = np.array([0, -1, 0])
+ v = np.array([0, 1, 1])
+ if inequality(v, n) != [0, -1, 0, -1]:
+ print("error in test_inequality")
+ else:
+ print("test_inequality successful")
+
def __gen_data():
- obj = load_obj('./hrp2/RL_com._reduced.obj')
- ineq = as_inequalities(obj)
- ok_points = [[0,0,0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
- not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]]
- return obj, ineq, ok_points, not_ok_points
+ obj = load_obj('./hrp2/RL_com._reduced.obj')
+ ineq = as_inequalities(obj)
+ ok_points = [[0, 0, 0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
+ not_ok_points = [[-0.3399, 0.2478, -0.722], [-0.1385, -0.4401, -0.1071]]
+ return obj, ineq, ok_points, not_ok_points
+
def test_belonging():
- data = __gen_data()
- ineq = data[1]
- ok_points = data[2]
- not_ok_points = data[3]
- for p in ok_points:
- assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
- for p in not_ok_points:
- assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
- print("test_belonging successful")
-
+ data = __gen_data()
+ ineq = data[1]
+ ok_points = data[2]
+ not_ok_points = data[3]
+ for p in ok_points:
+ assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
+ for p in not_ok_points:
+ assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
+ print("test_belonging successful")
+
+
def test_rotate_inequalities():
-
- tr = np.array([[ 1. , 0. , 0. , 0. ],
- [ 0. , 0.98006658, -0.19866933, 2. ],
- [ 0. , 0.19866933, 0.98006658, 0. ],
- [ 0. , 0. , 0. , 1. ]])
-
- data = __gen_data()
- ineq = rotate_inequalities(data[1], tr)
- ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[2]]
- not_ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[3]]
- for p in ok_points:
- assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
- for p in not_ok_points:
- assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
- print("test_rotate_inequalities successful")
-
+
+ tr = np.array([[1., 0., 0., 0.], [0., 0.98006658, -0.19866933, 2.], [0., 0.19866933, 0.98006658, 0.],
+ [0., 0., 0., 1.]])
+
+ data = __gen_data()
+ ineq = rotate_inequalities(data[1], tr)
+ ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[2]]
+ not_ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[3]]
+ for p in ok_points:
+ assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
+ for p in not_ok_points:
+ assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
+ print("test_rotate_inequalities successful")
+
def load_obj_and_save_ineq(in_name, out_name):
- obj = load_obj(in_name)
- ineq = as_inequalities(obj)
- ineq_to_file (ineq, out_name)
-
-load_obj_and_save_ineq('./lfleg_com_reduced.obj','./lfleg_com.ineq')
-load_obj_and_save_ineq('./lhleg_com_reduced.obj','./lhleg_com.ineq')
-load_obj_and_save_ineq('./rhleg_com_reduced.obj','./rhleg_com.ineq')
-load_obj_and_save_ineq('./rfleg_com_reduced.obj','./rfleg_com.ineq')
+ obj = load_obj(in_name)
+ ineq = as_inequalities(obj)
+ ineq_to_file(ineq, out_name)
+
+
+load_obj_and_save_ineq('./lfleg_com_reduced.obj', './lfleg_com.ineq')
+load_obj_and_save_ineq('./lhleg_com_reduced.obj', './lhleg_com.ineq')
+load_obj_and_save_ineq('./rhleg_com_reduced.obj', './rhleg_com.ineq')
+load_obj_and_save_ineq('./rfleg_com_reduced.obj', './rfleg_com.ineq')
diff --git a/script/relative_foot_positions/plot_polytopes.py b/script/relative_foot_positions/plot_polytopes.py
index 391207b0c1ab9ebfc9bb34f61c9f240c51fb83d1..322329df0aac9706121654d8c2785f650be7325a 100644
--- a/script/relative_foot_positions/plot_polytopes.py
+++ b/script/relative_foot_positions/plot_polytopes.py
@@ -1,66 +1,54 @@
import numpy as np
-from hpp_centroidal_dynamics import *
-from hpp_spline import *
-from numpy import array, asmatrix, matrix, zeros, ones
-from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
-from numpy.linalg import norm
-
-from scipy.spatial import ConvexHull
-from hpp_bezier_com_traj import *
-#~ from qp import solve_lp
-
-import eigenpy
-# ~ import cdd
-from curves import bezier3
-from random import random as rd
-from random import randint as rdi
-from numpy import squeeze, asarray
-
-eigenpy.switchToNumpyArray()
+# from hpp_centroidal_dynamics import *
+# from hpp_spline import *
+from numpy import array
+from constants_and_tools import genPolytope
+import matplotlib.pyplot as plt
-from constants_and_tools import *
-
-
+from scipy.spatial import ConvexHull
+# from hpp_bezier_com_traj import *
+# from qp import solve_lp
-import matplotlib.pyplot as plt
-from mpl_toolkits.mplot3d import Axes3D
+# import cdd
-def plot_hull_in_subplot(hull, pts, apts, ax, color = "r", just_pts = False):
+def plot_hull_in_subplot(hull, pts, apts, ax, color="r", just_pts=False):
# Plot defining corner points
ax.plot(apts.T[0], apts.T[1], apts.T[2], "ko")
if not just_pts:
for s in hull.simplices:
s = np.append(s, s[0]) # Here we cycle back to the first coordinate
- ax.plot(apts[s, 0], apts[s, 1], apts[s, 2], color+"-")
+ ax.plot(apts[s, 0], apts[s, 1], apts[s, 2], color + "-")
-def plot_hull(hull, pts, apts, color = "r", just_pts = False, plot = False, fig = None, ax = None):
+def plot_hull(hull, pts, apts, color="r", just_pts=False, plot=False, fig=None, ax=None):
if fig is None:
fig = plt.figure()
if ax is None:
- print ("ax is none")
+ print("ax is none")
ax = fig.add_subplot(111, projection="3d")
plot_hull_in_subplot(hull, pts, array(pts), ax, color, just_pts)
if plot:
- print (" PLOT" )
+ print(" PLOT")
plt.show(block=False)
return ax
-def plot_polytope_H_rep(A_in,b_in, color = "r", just_pts = False):
- hull, pts, apts, cd = genPolytope(A_in,b_in)
+
+def plot_polytope_H_rep(A_in, b_in, color="r", just_pts=False):
+ hull, pts, apts, cd = genPolytope(A_in, b_in)
plot_hull(hull, pts, apts, color, just_pts)
-def plot_polytope_V_rep(pts, color = "r", just_pts = False):
+
+def plot_polytope_V_rep(pts, color="r", just_pts=False):
pts = [array(el) for el in pts]
apts = array(pts)
hull = ConvexHull(apts, qhull_options='Q12')
plot_hull(hull, pts, apts, color, just_pts)
-
-def plot_polytope_CDD_PolyHeron(H, color = "r", just_pts = False):
+
+
+def plot_polytope_CDD_PolyHeron(H, color="r", just_pts=False):
g = H.get_generators()
pts = [array(g[el][1:]) for el in range(g.row_size)]
plot_polytope_V_rep(pts, color, just_pts)
-
diff --git a/script/relative_foot_positions/quaternion.py b/script/relative_foot_positions/quaternion.py
index 7d78f6cb1cab5da2cd5c42146c10b7ec77d7ad87..5eb6cfa7aa57ea08c76e43baddf0fd2a484159c7 100644
--- a/script/relative_foot_positions/quaternion.py
+++ b/script/relative_foot_positions/quaternion.py
@@ -18,7 +18,8 @@
import numpy as np
from numpy import linalg
-class Quaternion (object):
+
+class Quaternion(object):
"""
Quaternion class :
------------------
@@ -37,7 +38,7 @@ class Quaternion (object):
It can also return a rotation vector, a rotation matrix, or a SO3
(see the methods : to...() for more information).
"""
- def __init__(self,*args):
+ def __init__(self, *args):
"""
Instanciation of the quaternion with 1, 2 or 4 arguments :
-----------------------------------------------------------
@@ -86,104 +87,111 @@ class Quaternion (object):
e.g. : quat().fromRPY(R,P,Y)
"""
-
- error=False
- if len(args)==0: # By default, if no argument is given
- self.array=np.array([1.,0.,0.,0.])
- elif len (args) == 4: # From 4 elements
- if np.array(args).size==4:
- self.array = np.double(np.array (args))
+ error = False
+ if len(args) == 0: # By default, if no argument is given
+ self.array = np.array([1., 0., 0., 0.])
+ elif len(args) == 4: # From 4 elements
+ if np.array(args).size == 4:
+ self.array = np.double(np.array(args))
else:
- error=True
- elif len (args) == 1:
- if type(args[0])==Quaternion: # From a Quaternion
- self.array=args[0].array.copy()
- elif np.array(args[0]).size==1: # From one sized element, this element will be the scalar part, the vector part will be set at (0,0,0)
- self.array=np.double(np.hstack([np.array(args[0]),np.array([0,0,0])]))
- elif np.array(args[0]).size==4 and max(np.array(args[0]).shape)==4: # From an array, matrix, tuple or list of 4 elements
- self.array = np.double(np.array(args[0])).reshape(4,)
- elif np.array(args[0]).size==3 and max(np.array(args[0]).shape)==3: # From an array, matrix, tuple or list of 3 elements interpreted as a rotation vector
- rV=np.double(np.array(args[0])).reshape(3,)
- alpha=np.double(linalg.norm(rV))
- if alpha !=0:
- e=rV/alpha
+ error = True
+ elif len(args) == 1:
+ if type(args[0]) == Quaternion: # From a Quaternion
+ self.array = args[0].array.copy()
+ elif np.array(args[0]).size == 1:
+ # From one sized element, this element will be the scalar part,
+ # the vector part will be set at (0,0,0)
+ self.array = np.double(np.hstack([np.array(args[0]), np.array([0, 0, 0])]))
+ elif np.array(args[0]).size == 4 and max(np.array(
+ args[0]).shape) == 4: # From an array, matrix, tuple or list of 4 elements
+ self.array = np.double(np.array(args[0])).reshape(4, )
+ elif np.array(args[0]).size == 3 and max(
+ np.array(args[0]).shape
+ ) == 3: # From an array, matrix, tuple or list of 3 elements interpreted as a rotation vector
+ rV = np.double(np.array(args[0])).reshape(3, )
+ alpha = np.double(linalg.norm(rV))
+ if alpha != 0:
+ e = rV / alpha
else:
- e=rV
- self.array=np.hstack([np.cos(alpha/2.),np.sin(alpha/2.)*e])
- elif len(np.array(args[0]).shape)==2 and np.array(args[0]).shape[0]>=3 and np.array(args[0]).shape[1]>=3: # From a to 2 dimension array convertible array, matrix, tuple or list with at least (3*3) elements interpreted as a rotation matrix
- rM=np.double(np.array(args[0])[:3,:3])
- selec=np.zeros(4)
- selec[0]=1+rM[0,0]+rM[1,1]+rM[2,2]
- selec[1]=1+rM[0,0]-rM[1,1]-rM[2,2]
- selec[2]=1-rM[0,0]+rM[1,1]-rM[2,2]
- selec[3]=1-rM[0,0]-rM[1,1]+rM[2,2]
- param=selec.argmax()
- if selec[param]>0:
- q=np.zeros(4)
- if param==0:
- q[0]=np.sqrt(selec[param])
- q[1]=(rM[2,1]-rM[1,2])/q[0]
- q[2]=(rM[0,2]-rM[2,0])/q[0]
- q[3]=(rM[1,0]-rM[0,1])/q[0]
- self.array=q*0.5
+ e = rV
+ self.array = np.hstack([np.cos(alpha / 2.), np.sin(alpha / 2.) * e])
+ elif len(np.array(
+ args[0]).shape) == 2 and np.array(args[0]).shape[0] >= 3 and np.array(args[0]).shape[1] >= 3:
+ # From a to 2 dimension array convertible array, matrix, tuple or list with at least (3*3)
+ # elements interpreted as a rotation matrix
+ rM = np.double(np.array(args[0])[:3, :3])
+ selec = np.zeros(4)
+ selec[0] = 1 + rM[0, 0] + rM[1, 1] + rM[2, 2]
+ selec[1] = 1 + rM[0, 0] - rM[1, 1] - rM[2, 2]
+ selec[2] = 1 - rM[0, 0] + rM[1, 1] - rM[2, 2]
+ selec[3] = 1 - rM[0, 0] - rM[1, 1] + rM[2, 2]
+ param = selec.argmax()
+ if selec[param] > 0:
+ q = np.zeros(4)
+ if param == 0:
+ q[0] = np.sqrt(selec[param])
+ q[1] = (rM[2, 1] - rM[1, 2]) / q[0]
+ q[2] = (rM[0, 2] - rM[2, 0]) / q[0]
+ q[3] = (rM[1, 0] - rM[0, 1]) / q[0]
+ self.array = q * 0.5
# print '--1--V3'
- elif param==1:
- q[1]=np.sqrt(selec[param])
- q[0]=(rM[2,1]-rM[1,2])/q[1]
- q[2]=(rM[1,0]+rM[0,1])/q[1]
- q[3]=(rM[0,2]+rM[2,0])/q[1]
- self.array=q*0.5
+ elif param == 1:
+ q[1] = np.sqrt(selec[param])
+ q[0] = (rM[2, 1] - rM[1, 2]) / q[1]
+ q[2] = (rM[1, 0] + rM[0, 1]) / q[1]
+ q[3] = (rM[0, 2] + rM[2, 0]) / q[1]
+ self.array = q * 0.5
# print '--2--V3'
- elif param==2:
- q[2]=np.sqrt(selec[param])
- q[0]=(rM[0,2]-rM[2,0])/q[2]
- q[1]=(rM[1,0]+rM[0,1])/q[2]
- q[3]=(rM[2,1]+rM[1,2])/q[2]
- self.array=q*0.5
+ elif param == 2:
+ q[2] = np.sqrt(selec[param])
+ q[0] = (rM[0, 2] - rM[2, 0]) / q[2]
+ q[1] = (rM[1, 0] + rM[0, 1]) / q[2]
+ q[3] = (rM[2, 1] + rM[1, 2]) / q[2]
+ self.array = q * 0.5
# print '--3--V3'
- elif param==3:
- q[3]=np.sqrt(selec[param])
- q[0]=(rM[1,0]-rM[0,1])/q[3]
- q[1]=(rM[0,2]+rM[2,0])/q[3]
- q[2]=(rM[2,1]+rM[1,2])/q[3]
- self.array=q*0.5
+ elif param == 3:
+ q[3] = np.sqrt(selec[param])
+ q[0] = (rM[1, 0] - rM[0, 1]) / q[3]
+ q[1] = (rM[0, 2] + rM[2, 0]) / q[3]
+ q[2] = (rM[2, 1] + rM[1, 2]) / q[3]
+ self.array = q * 0.5
# print '--4--V3'
else:
- error=True
+ error = True
else:
- error=True
- elif len(args)==2: # From a scalar part (1 element) and a vector part (3 elements)
- arg0=np.double(np.array(args[0]))
- arg1=np.double(np.array(args[1]))
- if arg0.size==1 and arg1.size==3:
- self.array=np.zeros(4)
- self.array[0]=arg0
- self.array[1:4]=arg1[:]
- elif arg0.size==3 and arg1.size==1:
- self.array=np.zeros(4)
- self.array[0]=arg1
- self.array[1:4]=arg0[:]
+ error = True
+ elif len(args) == 2: # From a scalar part (1 element) and a vector part (3 elements)
+ arg0 = np.double(np.array(args[0]))
+ arg1 = np.double(np.array(args[1]))
+ if arg0.size == 1 and arg1.size == 3:
+ self.array = np.zeros(4)
+ self.array[0] = arg0
+ self.array[1:4] = arg1[:]
+ elif arg0.size == 3 and arg1.size == 1:
+ self.array = np.zeros(4)
+ self.array[0] = arg1
+ self.array[1:4] = arg0[:]
else:
- error=True
+ error = True
else:
- error=True
+ error = True
- if error==False and self.array.shape!=(4,):
+ if not error and self.array.shape != (4, ):
del self.array
- error=True
+ error = True
if error:
- raise TypeError ("Impossible to instanciate the Quaternion object with the given arguments")
+ raise TypeError("Impossible to instanciate the Quaternion object with the given arguments")
def __str__(self):
"""
String representation of the quaternion.
"""
- aff='[ '
- aff+=str(self.array [0])+' + '
- aff+=str(self.array [1])+' i + '
- aff+=str(self.array [2])+' j + '
- aff+=str(self.array [3])+' k ]'
+ aff = '[ '
+ aff += str(self.array[0]) + ' + '
+ aff += str(self.array[1]) + ' i + '
+ aff += str(self.array[2]) + ' j + '
+ aff += str(self.array[3]) + ' k ]'
return aff
def __neg__(self):
@@ -193,56 +201,57 @@ class Quaternion (object):
"""
return Quaternion(-self.array)
- def __add__(self,other):
+ def __add__(self, other):
"""
If other is not a quaternion it is casted to a quaternion,
the elements are added one to one.
"""
- if type(other)!=Quaternion:
- q2=Quaternion(other)
+ if type(other) != Quaternion:
+ q2 = Quaternion(other)
else:
- q2=other
- return Quaternion(self.array+q2.array)
+ q2 = other
+ return Quaternion(self.array + q2.array)
- def __sub__(self,other):
+ def __sub__(self, other):
"""
If other is not a quaternion it is casted to a quaternion,
the elements are substracted one to one.
"""
- if type(other)!=Quaternion:
- q2=Quaternion(other)
+ if type(other) != Quaternion:
+ q2 = Quaternion(other)
else:
- q2=other
- return Quaternion(self.array-q2.array)
+ q2 = other
+ return Quaternion(self.array - q2.array)
- def __mul__(self,other):
+ def __mul__(self, other):
"""
If other is not a quaternion it is casted to a quaternion,
the result of the quaternion multiplication is returned.
"""
- if type(other)!=Quaternion:
- q2=Quaternion(other)
+ if type(other) != Quaternion:
+ q2 = Quaternion(other)
else:
- q2=other
- qr=np.zeros(4)
- qr[0]=self.array[0]*q2.array[0]-np.vdot(self.array[1:],q2.array[1:])
- qr[1:4]=np.cross(self.array[1:4],q2.array[1:4])+self.array[0]*q2.array[1:4]+q2.array[0]*self.array[1:4]
+ q2 = other
+ qr = np.zeros(4)
+ qr[0] = self.array[0] * q2.array[0] - np.vdot(self.array[1:], q2.array[1:])
+ qr[1:4] = np.cross(self.array[1:4],
+ q2.array[1:4]) + self.array[0] * q2.array[1:4] + q2.array[0] * self.array[1:4]
return Quaternion(qr)
- def __rmul__(self,other):
+ def __rmul__(self, other):
"""
other is casted to a quaternion,
the result of the quaternion multiplication is returned.
"""
- return Quaternion(other)*self
+ return Quaternion(other) * self
- def transform (self, v):
+ def transform(self, v):
"""
apply rotation to a vector
"""
- u = np.array (self.array [1:4])
- s = self.array [0]
- return 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*np.cross(u, v)
+ u = np.array(self.array[1:4])
+ s = self.array[0]
+ return 2 * u.dot(v) * u + (s * s - u.dot(u)) * v + 2 * s * np.cross(u, v)
def __abs__(self):
"""
@@ -254,50 +263,50 @@ class Quaternion (object):
"""
Returns the conjugate of the quaternion.
"""
- return Quaternion(self.array[0],-self.array[1:4])
+ return Quaternion(self.array[0], -self.array[1:4])
def inv(self):
"""
Returns the inverse of the quaternion.
"""
- return Quaternion(self.conjugate().array/(abs(self)**2))
+ return Quaternion(self.conjugate().array / (abs(self)**2))
- def __div__(self,other):
+ def __div__(self, other):
"""
If other is not a quaternion it is casted to a quaternion,
the result of the quaternion multiplication with the inverse of other
is returned.
"""
- if type(other)!=Quaternion:
- q2=Quaternion(other)
+ if type(other) != Quaternion:
+ q2 = Quaternion(other)
else:
- q2=other
- return self*q2.inv()
+ q2 = other
+ return self * q2.inv()
- def __pow__(self,n):
+ def __pow__(self, n):
"""
Returns quaternion**n with quaternion**0 = Quaternion(1,0,0,0).
"""
- r=Quaternion()
+ r = Quaternion()
for i in range(n):
- r=r*self
+ r = r * self
return r
- def normalize (self):
+ def normalize(self):
"""
Changes the values of the quaternion to make it a unit quaternion
representing the same rotation as the original one
and returns the updated version.
"""
- self.array /= abs(self);
+ self.array /= abs(self)
return self
- def normalized (self):
+ def normalized(self):
"""
Returns the unit quaternion representation of the quaternion
without changing the original.
"""
- qr=Quaternion(self)
+ qr = Quaternion(self)
qr.normalize()
return qr
@@ -306,17 +315,17 @@ class Quaternion (object):
Returns a (3*3) array (rotation matrix)
representing the same rotation as the (normalized) quaternion.
"""
- q=self.normalized().array
- rm=np.zeros((3,3))
- rm[0,0]=1-2*(q[2]**2+q[3]**2)
- rm[0,1]=2*q[1]*q[2]-2*q[0]*q[3]
- rm[0,2]=2*q[1]*q[3]+2*q[0]*q[2]
- rm[1,0]=2*q[1]*q[2]+2*q[0]*q[3]
- rm[1,1]=1-2*(q[1]**2+q[3]**2)
- rm[1,2]=2*q[2]*q[3]-2*q[0]*q[1]
- rm[2,0]=2*q[1]*q[3]-2*q[0]*q[2]
- rm[2,1]=2*q[2]*q[3]+2*q[0]*q[1]
- rm[2,2]=1-2*(q[1]**2+q[2]**2)
+ q = self.normalized().array
+ rm = np.zeros((3, 3))
+ rm[0, 0] = 1 - 2 * (q[2]**2 + q[3]**2)
+ rm[0, 1] = 2 * q[1] * q[2] - 2 * q[0] * q[3]
+ rm[0, 2] = 2 * q[1] * q[3] + 2 * q[0] * q[2]
+ rm[1, 0] = 2 * q[1] * q[2] + 2 * q[0] * q[3]
+ rm[1, 1] = 1 - 2 * (q[1]**2 + q[3]**2)
+ rm[1, 2] = 2 * q[2] * q[3] - 2 * q[0] * q[1]
+ rm[2, 0] = 2 * q[1] * q[3] - 2 * q[0] * q[2]
+ rm[2, 1] = 2 * q[2] * q[3] + 2 * q[0] * q[1]
+ rm[2, 2] = 1 - 2 * (q[1]**2 + q[2]**2)
return rm
def toRotationVector(self):
@@ -324,11 +333,11 @@ class Quaternion (object):
Returns a 3-sized array (rotation vector)
representing the same rotation as the (normalized) quaternion.
"""
- q=self.normalized().array
- rV=np.zeros(3)
- alpha=2*np.arccos(q[0])
- if linalg.norm(q[1:4])!=0:
- rV=alpha*q[1:4]/linalg.norm(q[1:4])
+ q = self.normalized().array
+ rV = np.zeros(3)
+ alpha = 2 * np.arccos(q[0])
+ if linalg.norm(q[1:4]) != 0:
+ rV = alpha * q[1:4] / linalg.norm(q[1:4])
return rV
def copy(self):
@@ -353,13 +362,15 @@ class Quaternion (object):
followed by a rotation of P about the new y-axis,
followed by a rotation of R about the new x-axis.
"""
- q=self.normalized().array
- r=np.arctan2(2*(q[0]*q[1]+q[2]*q[3]),1-2*(q[1]**2+q[2]**2))
- p=np.arctan2(2*(q[0]*q[2]-q[3]*q[1]),np.sqrt((2*(q[0]*q[1]+q[2]*q[3]))**2+(1-2*(q[1]**2+q[2]**2))**2)) # We cas use arcsin but arctan2 is more robust
- y=np.arctan2(2*(q[0]*q[3]+q[1]*q[2]),1-2*(q[2]**2+q[3]**2))
- return np.array([r,p,y])
+ q = self.normalized().array
+ r = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1]**2 + q[2]**2))
+ p = np.arctan2(2 * (q[0] * q[2] - q[3] * q[1]),
+ np.sqrt((2 * (q[0] * q[1] + q[2] * q[3]))**2 +
+ (1 - 2 * (q[1]**2 + q[2]**2))**2)) # We cas use arcsin but arctan2 is more robust
+ y = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2]**2 + q[3]**2))
+ return np.array([r, p, y])
- def fromRPY(self,R,P,Y):
+ def fromRPY(self, R, P, Y):
"""
Set the values of the quaternion to the values of a unit quaternion
representing the same rotation as the one performed by Roll Pitch Yaw :
@@ -370,17 +381,17 @@ class Quaternion (object):
followed by a rotation of P about the new y-axis,
followed by a rotation of R about the new x-axis.
"""
- r=R/2.
- p=P/2.
- y=Y/2.
- self.array[0]=np.cos(r)*np.cos(p)*np.cos(y)+np.sin(r)*np.sin(p)*np.sin(y)
- self.array[1]=np.sin(r)*np.cos(p)*np.cos(y)-np.cos(r)*np.sin(p)*np.sin(y)
- self.array[2]=np.cos(r)*np.sin(p)*np.cos(y)+np.sin(r)*np.cos(p)*np.sin(y)
- self.array[3]=np.cos(r)*np.cos(p)*np.sin(y)-np.sin(r)*np.sin(p)*np.cos(y)
+ r = R / 2.
+ p = P / 2.
+ y = Y / 2.
+ self.array[0] = np.cos(r) * np.cos(p) * np.cos(y) + np.sin(r) * np.sin(p) * np.sin(y)
+ self.array[1] = np.sin(r) * np.cos(p) * np.cos(y) - np.cos(r) * np.sin(p) * np.sin(y)
+ self.array[2] = np.cos(r) * np.sin(p) * np.cos(y) + np.sin(r) * np.cos(p) * np.sin(y)
+ self.array[3] = np.cos(r) * np.cos(p) * np.sin(y) - np.sin(r) * np.sin(p) * np.cos(y)
return self.normalize()
- def toTuple (self):
+ def toTuple(self):
"""
Return quaternion as a tuple a float starting with real part.
"""
- return tuple (self.array)
+ return tuple(self.array)
diff --git a/script/relative_foot_positions/reduce.py b/script/relative_foot_positions/reduce.py
index bded2e164c3be418efcb654b14f88a9ac16b82c8..1e0b55f695a1098d7722e0ed3734d8e748a1c9ff 100644
--- a/script/relative_foot_positions/reduce.py
+++ b/script/relative_foot_positions/reduce.py
@@ -1,67 +1,82 @@
import bpy
-
+import glob
+import os
# this script is tested with blender 2.82
# WARNING !! this script will erase your scene
-#
+#
-#change those parameters according to your needs
+# change those parameters according to your needs
TARGET_NUM_FACES = 24.
FOLDER_PATH = "/media/data/dev/linux/hpp/src/anymal-rbprm/script/relative_foot_positions/"
-OUTPUT_PATH = FOLDER_PATH+"output/"
+OUTPUT_PATH = FOLDER_PATH + "output/"
EXTENSION = ".obj"
+
def decimate(obj):
nFaces = len(obj.data.polygons)
- heuristic_ratio = TARGET_NUM_FACES / float(nFaces)
- bpy.ops.mesh.decimate(ratio=heuristic_ratio)
- return
+ heuristic_ratio = TARGET_NUM_FACES / float(nFaces)
+ bpy.ops.mesh.decimate(ratio=heuristic_ratio)
+ return
+
def load_obj(file):
# ~ bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file, axis_forward='X', axis_up='Z')
- bpy.ops.import_scene.obj(filepath=FOLDER_PATH+file)
+ bpy.ops.import_scene.obj(filepath=FOLDER_PATH + file)
obj = bpy.data.objects[-1]
- #api change in 2.82
- #bpy.context.scene.objects.active = obj
+ # api change in 2.82
+ # bpy.context.scene.objects.active = obj
bpy.context.view_layer.objects.active = obj
bpy.ops.object.editmode_toggle()
- bpy.ops.mesh.delete( type='EDGE_FACE')
+ bpy.ops.mesh.delete(type='EDGE_FACE')
bpy.ops.mesh.select_mode(type="VERT")
- bpy.ops.mesh.select_all(action = 'SELECT')
+ bpy.ops.mesh.select_all(action='SELECT')
bpy.ops.mesh.convex_hull()
decimate(obj)
-
- #to export first extract filename
+
+ # to export first extract filename
idx = file.index(EXTENSION)
obj.name = file[:idx] + "_reduced"
- bpy.ops.export_scene.obj(filepath=OUTPUT_PATH+obj.name+EXTENSION, check_existing=True, filter_glob="*.obj;*.mtl",
- use_selection=True, use_animation=False,
- use_mesh_modifiers=True, use_edges=True,
- use_smooth_groups=False, use_smooth_groups_bitflags=False,
- use_normals=True, use_uvs=True, use_materials=False,
- use_triangles=False, use_nurbs=False,
- use_vertex_groups=False, use_blen_objects=True,
- group_by_object=False, group_by_material=False, keep_vertex_order=False,
- # ~ global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
- global_scale=1.0, path_mode='AUTO')
-
- #delete all objects
- bpy.ops.object.delete()
-
-
+ bpy.ops.export_scene.obj(
+ filepath=OUTPUT_PATH + obj.name + EXTENSION,
+ check_existing=True,
+ filter_glob="*.obj;*.mtl",
+ use_selection=True,
+ use_animation=False,
+ use_mesh_modifiers=True,
+ use_edges=True,
+ use_smooth_groups=False,
+ use_smooth_groups_bitflags=False,
+ use_normals=True,
+ use_uvs=True,
+ use_materials=False,
+ use_triangles=False,
+ use_nurbs=False,
+ use_vertex_groups=False,
+ use_blen_objects=True,
+ group_by_object=False,
+ group_by_material=False,
+ keep_vertex_order=False,
+ # ~ global_scale=1.0, path_mode='AUTO', axis_forward='X', axis_up='Z')
+ global_scale=1.0,
+ path_mode='AUTO')
+
+ # delete all objects
+ bpy.ops.object.delete()
+
+
bpy.ops.object.select_all(action='SELECT')
-bpy.ops.object.delete()
-
-import glob, os
+bpy.ops.object.delete()
+
os.chdir(FOLDER_PATH)
directory = os.path.dirname(OUTPUT_PATH)
if not os.path.exists(directory):
os.makedirs(directory)
-#clear the scene !
+# clear the scene !
for file in glob.glob("*.obj"):
load_obj(file)
diff --git a/script/relative_foot_positions/relativeFootPositionQuasiFlat.py b/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
index 01ebc1d306726560db979ed488b763c5293f2768..42d995c09ef78a47e3e4aa0557e411e07d8cbcd9 100644
--- a/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
+++ b/script/relative_foot_positions/relativeFootPositionQuasiFlat.py
@@ -1,16 +1,16 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
-from hpp.gepetto import Viewer
+# from numpy.linalg import norm
+# import numpy as np
+from numpy import array, zeros, ones
+from scipy.spatial import ConvexHull
+from scipy.optimize import linprog
from hpp.gepetto import ViewerFactory
-from numpy import array
-from numpy.linalg import norm
-
-
+from hpp.corbaserver.rbprm import rbprmstate, state_alg
from hpp.corbaserver.rbprm.anymal import Robot
-from hpp.corbaserver.rbprm.tools.display_tools import *
+from hpp.corbaserver import ProblemSolver
+from hpp.corbaserver.rbprm.tools.display_tools import hull_to_obj, plt, plot_hull
-from plot_polytopes import *
-from pinocchio import Quaternion
+# from plot_polytopes import *
+# from pinocchio import Quaternion
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
@@ -23,30 +23,30 @@ MIN_HEIGHT_COM = 0.3
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05
-
-fullBody = Robot ()
-
-
+fullBody = Robot()
fullBody.setConstrainedJointsBounds()
-fullBody.setJointBounds("LF_KFE",[-1.4,0.])
-fullBody.setJointBounds("RF_KFE",[-1.4,0.])
-fullBody.setJointBounds("LH_KFE",[0.,1.4])
-fullBody.setJointBounds("RH_KFE",[0.,1.4])
-fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
-dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
-fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=12)
-
-#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-from hpp.corbaserver import ProblemSolver
+fullBody.setJointBounds("LF_KFE", [-1.4, 0.])
+fullBody.setJointBounds("RF_KFE", [-1.4, 0.])
+fullBody.setJointBounds("LH_KFE", [0., 1.4])
+fullBody.setJointBounds("RH_KFE", [0., 1.4])
+fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
+dict_heuristic = {
+ fullBody.rLegId: "static",
+ fullBody.lLegId: "static",
+ fullBody.rArmId: "fixedStep04",
+ fullBody.lArmId: "fixedStep04"
+}
+fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)
+
nbSamples = 1
-ps = ProblemSolver( fullBody )
-vf = ViewerFactory (ps)
+ps = ProblemSolver(fullBody)
+vf = ViewerFactory(ps)
v = vf.createViewer()
rootName = 'root_joint'
-zero = [0.,0.,0.]
+zero = [0., 0., 0.]
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
@@ -56,117 +56,123 @@ rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]
lLegId = fullBody.lLegId
-lLeg = fullBody.lleg
-lfoot = fullBody.lfoot
+lLeg = fullBody.lleg
+lfoot = fullBody.lfoot
-#make sure this is 0
-q_0 = fullBody.getCurrentConfig ()
-zeroConf = [0,0,0, 0, 0, 0, 1.]
+# make sure this is 0
+q_0 = fullBody.getCurrentConfig()
+zeroConf = [0, 0, 0, 0, 0, 0, 1.]
q_0[0:7] = zeroConf
-fullBody.setCurrentConfig (q_0)
-
-effectors = [fullBody.rfoot, fullBody.lfoot, fullBody.rhand, fullBody.lhand,]
+fullBody.setCurrentConfig(q_0)
+
+effectors = [
+ fullBody.rfoot,
+ fullBody.lfoot,
+ fullBody.rhand,
+ fullBody.lhand,
+]
limbIds = [fullBody.rLegId, fullBody.lLegId, fullBody.rArmId, fullBody.lArmId]
offsets = [array(rLegOffset), array(lLegOffset), array(rArmOffset), array(lArmOffset)]
-import numpy as np
-
compoints = [[] for _ in effectors]
-#~ compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
-points = [ {} for _ in effectors]
-for i, eff in enumerate(effectors):
+# compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],
+# [[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
+points = [{} for _ in effectors]
+for i, eff in enumerate(effectors):
for j, otherEff in enumerate(effectors):
if i != j:
points[i][otherEff] = []
-
success = 0
fails = 0
-from hpp.corbaserver.rbprm import rbprmstate, state_alg
-from scipy.spatial import ConvexHull
-from scipy.optimize import linprog
-#static eq is com is convex combination of pos (projected)
+# static eq is com is convex combination of pos (projected)
def staticEq(positions, com):
sizeX = len(positions)
- E = zeros((3,sizeX))
+ E = zeros((3, sizeX))
for i, pos in enumerate(positions):
- E[:2,i] = pos[:2]
+ E[:2, i] = pos[:2]
e = array([com[0], com[1], 1.])
- E[2,:] = ones(sizeX)
- try:
- res = linprog(ones(sizeX), A_ub=None, b_ub=None, A_eq=E, b_eq=e, bounds=[(0.,1.) for _ in range(sizeX)], method='interior-point', callback=None, options={'presolve': True})
- return res['success']
- except:
- return False
-
-
-#returns true of one of the point is inside the convex hulls of the others. We do not want that
+ E[2, :] = ones(sizeX)
+ res = linprog(ones(sizeX),
+ A_ub=None,
+ b_ub=None,
+ A_eq=E,
+ b_eq=e,
+ bounds=[(0., 1.) for _ in range(sizeX)],
+ method='interior-point',
+ callback=None,
+ options={'presolve': True})
+ return res['success']
+
+
+# returns true of one of the point is inside the convex hulls of the others. We do not want that
def pointInsideHull(positions):
for i, pos in enumerate(positions):
- others = positions[:i] + positions[i+1:]
+ others = positions[:i] + positions[i + 1:]
if staticEq(others, pos):
return True
return False
-def genFlat(init = False):
- q = fullBody.shootRandomConfig()
- if init:
- q = fullBody.referenceConfig[::]
- q[0:7] = zeroConf
+
+def genFlat(init=False):
+ q = fullBody.shootRandomConfig()
+ if init:
+ q = fullBody.referenceConfig[::]
+ q[0:7] = zeroConf
+ fullBody.setCurrentConfig(q)
+ # v(q)
+
+ positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
+
+ s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds)
+ succ = True
+ for effId, pos in zip(limbIds, positions):
+ s, succ = state_alg.addNewContact(s, effId, pos, [0., 0., 1.], num_max_sample=0)
+ if not succ:
+ break
+
+ # posrf = fullBody.getJointPosition(rfoot)[:3]
+ # poslf = fullBody.getJointPosition(lfoot)[:3]
+ # print ("limbsIds ", limbIds)
+ # s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
+ # s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
+ # if succ:
+ # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
+ if succ:
+ # ~ succ = fullBody.isConfigValid(q)[0] and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
+ succ = fullBody.isConfigValid(q)[0]
+
+ # assert that in static equilibrium
+ if succ:
fullBody.setCurrentConfig(q)
- #~ v(q)
-
- positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
-
- s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
- succ = True
- for effId, pos in zip(limbIds,positions):
- s, succ = state_alg.addNewContact(s, effId, pos, [0.,0.,1.], num_max_sample = 0)
- if not succ:
- break
-
- # ~ posrf = fullBody.getJointPosition(rfoot)[:3]
- # ~ poslf = fullBody.getJointPosition(lfoot)[:3]
- # ~ print ("limbsIds ", limbIds)
- # ~ s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
- # ~ s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
- # ~ if succ:
- # ~ s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
- if succ:
- # ~ succ = fullBody.isConfigValid(q)[0] and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
- succ = fullBody.isConfigValid(q)[0]
-
- #assert that in static equilibrium
- if succ:
- fullBody.setCurrentConfig(q)
- succ = staticEq(positions, fullBody.getCenterOfMass())
- if not succ:
- v(q)
- if succ:
- succ = not pointInsideHull(positions)
- if not succ:
- print ("************* contacts crossing", not succ)
- v(q)
- #~ if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
- # ~ if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
- v(s.q())
- return s.q(), succ, s, positions
-
-
+ succ = staticEq(positions, fullBody.getCenterOfMass())
+ if not succ:
+ v(q)
+ if succ:
+ succ = not pointInsideHull(positions)
+ if not succ:
+ print("************* contacts crossing", not succ)
+ v(q)
+ # if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
+ # if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
+ v(s.q())
+ return s.q(), succ, s, positions
+
+
def printFootPositionRelativeToOther(nbConfigs):
for i in range(0, nbConfigs):
if i > 0 and not i % IT_DISPLAY_PROGRESS:
print(int((i * 100) / nbConfigs), " % done")
- q, succ, s, pos = genFlat(i==0)
+ q, succ, s, pos = genFlat(i == 0)
if succ:
global success
success += 1
addCom = True
for j, effectorName in enumerate(effectors):
- for otheridx, (oeffectorName, limbId) in enumerate(zip(effectors,limbIds)):
- if otheridx != j:
+ for otheridx, (oeffectorName, limbId) in enumerate(zip(effectors, limbIds)):
+ if otheridx != j:
fullBody.setCurrentConfig(q)
pos_other = fullBody.getJointPosition(oeffectorName)
pos = fullBody.getJointPosition(effectorName)
@@ -182,40 +188,41 @@ def printFootPositionRelativeToOther(nbConfigs):
# ~ p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
# ~ rm = np.zeros((4, 4))
# ~ for k in range(0, 3):
- # ~ for l in range(0, 3):
- # ~ rm[k, l] = rot[k, l]
+ # ~ for l in range(0, 3):
+ # ~ rm[k, l] = rot[k, l]
# ~ for m in range(0, 3):
- # ~ rm[m, 3] = qEffector[m]
+ # ~ rm[m, 3] = qEffector[m]
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
- # ~ p = invrm.dot([0, 0, 0., 1])
- if (MAX_DIST_BETWEEN_FEET_Z > abs(p[2])):
- if (MIN_DIST_BETWEEN_FEET_Y <= abs(p[1])):
- if (MIN_DIST_BETWEEN_FEET_X <= abs(p[0])): #this is not what we want to do in theory but it works well in fact
+ # ~ p = invrm.dot([0, 0, 0., 1])
+ if (MAX_DIST_BETWEEN_FEET_Z > abs(p[2])):
+ if (MIN_DIST_BETWEEN_FEET_Y <= abs(p[1])):
+ if (MIN_DIST_BETWEEN_FEET_X <= abs(p[0])):
+ # this is not what we want to do in theory but it works well in fact
points[j][oeffectorName].append(p[:3])
else:
addCom = False
else:
addCom = False
else:
- print ('rejecting ',effectorName, ' ', oeffectorName , p, abs(p[2]))
+ print('rejecting ', effectorName, ' ', oeffectorName, p, abs(p[2]))
# ~ print ('pos_other', pos_other)
# ~ print ('old_pos', old_pos)
addCom = False
v(q)
# ~ if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
- # ~ points[j].append(p[:3])
+ # ~ points[j].append(p[:3])
# ~ elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
- # ~ points[j].append(p[:3])
+ # ~ points[j].append(p[:3])
# ~ else:
- # ~ addCom =
+ # ~ addCom =
# now compute coms
fullBody.setCurrentConfig(q)
com = array(fullBody.getCenterOfMass())
- print ('com ', com)
+ print('com ', com)
# ~ for x in range(0, 3):
- # ~ q[x] = -com[x]
+ # ~ q[x] = -com[x]
for j, effectorName in enumerate(effectors):
pos = fullBody.getJointPosition(effectorName)
rp = array(com) - array(pos[:3]).tolist()
@@ -225,10 +232,10 @@ def printFootPositionRelativeToOther(nbConfigs):
# ~ p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
# ~ rm = np.zeros((4, 4))
# ~ for k in range(0, 3):
- # ~ for l in range(0, 3):
- # ~ rm[k, l] = rot[k, l]
+ # ~ for l in range(0, 3):
+ # ~ rm[k, l] = rot[k, l]
# ~ for m in range(0, 3):
- # ~ rm[m, 3] = qEffector[m]
+ # ~ rm[m, 3] = qEffector[m]
# ~ rm[3, 3] = 1
# ~ invrm = np.linalg.inv(rm)
# ~ p = invrm.dot([0, 0, 0, 1])
@@ -237,16 +244,15 @@ def printFootPositionRelativeToOther(nbConfigs):
if (rp[2] < MIN_HEIGHT_COM):
addCom = False
- print ("reject min heught")
+ print("reject min heught")
if addCom:
compoints[j].append(rp)
# ~ if j == 1:
- # ~ if rp[1] < MARGIN_FEET_SIDE:
- # ~ compoints[j].append(rp)
+ # ~ if rp[1] < MARGIN_FEET_SIDE:
+ # ~ compoints[j].append(rp)
# ~ else:
- # ~ if rp[1] > -MARGIN_FEET_SIDE:
- # ~ compoints[j].append(rp)
-
+ # ~ if rp[1] > -MARGIN_FEET_SIDE:
+ # ~ compoints[j].append(rp)
else:
global fails
@@ -257,28 +263,27 @@ def printFootPositionRelativeToOther(nbConfigs):
# for p in points[j]:
# f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
# f1.close()
-
-s = rbprmstate.State(fullBody, q = fullBody.getCurrentConfig(), limbsIncontact = [fullBody.limbs_names[0]])
-#~ printRootPosition(rLegId, rfoot, nbSamples)
-#~ printRootPosition(lLegId, lfoot, nbSamples)
-#~ printRootPosition(rarmId, rHand, nbSamples)
-#~ printRootPosition(larmId, lHand, nbSamples)
-printFootPositionRelativeToOther(6000)
-print ("successes ", success )
-print ("fails ", fails )
+s = rbprmstate.State(fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]])
+# printRootPosition(rLegId, rfoot, nbSamples)
+# printRootPosition(lLegId, lfoot, nbSamples)
+# printRootPosition(rarmId, rHand, nbSamples)
+# printRootPosition(larmId, lHand, nbSamples)
+printFootPositionRelativeToOther(6000)
+print("successes ", success)
+print("fails ", fails)
# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:], compoints[:1], points[:1]):
-for effector, limbId, comData, pointsData in zip(effectors,limbIds, compoints, points):
+for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints, points):
hcom = ConvexHull(comData)
- hull_to_obj(hcom,comData,"anymal_COM_constraints_in_"+str(limbId)+"_effector_frame_quasi_static.obj")
+ hull_to_obj(hcom, comData, "anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj")
fig = plt.figure()
- fig.suptitle("anymal_COM_constraints_in_"+str(limbId)+"_effector_frame_quasi_static.obj", fontsize=16)
- plot_hull(hcom, comData, array(comData), color = "r", plot = False, fig = fig, ax = None)
-
+ fig.suptitle("anymal_COM_constraints_in_" + str(limbId) + "_effector_frame_quasi_static.obj", fontsize=16)
+ plot_hull(hcom, comData, array(comData), color="r", plot=False, fig=fig, ax=None)
+
fig = plt.figure()
fig.suptitle(str(limbId), fontsize=16)
# ~ axes = [221,222,223,224]
@@ -287,13 +292,18 @@ for effector, limbId, comData, pointsData in zip(effectors,limbIds, compoints, p
for (oEffector, pts) in pointsData.items():
# ~ ax = fig.add_subplot(axId, projection="3d")
hpts = ConvexHull(pts)
- hull_to_obj(hpts,pts,"anymal_"+str(oEffector)+"_constraints_in_" +str(limbId)+".obj")
- print ("ax ", ax)
- ax = plot_hull(hpts, pts, array(pts), color = "b", plot = False, fig = fig, ax = ax)
- print("effector ", limbId, )
- print("oEffector ", oEffector, )
- plt.show(block = False)
-
+ hull_to_obj(hpts, pts, "anymal_" + str(oEffector) + "_constraints_in_" + str(limbId) + ".obj")
+ print("ax ", ax)
+ ax = plot_hull(hpts, pts, array(pts), color="b", plot=False, fig=fig, ax=ax)
+ print(
+ "effector ",
+ limbId,
+ )
+ print(
+ "oEffector ",
+ oEffector,
+ )
+ plt.show(block=False)
# ~ hcomRF = ConvexHull(compoints[0])
# ~ hcomLF = ConvexHull(compoints[1])
@@ -305,10 +315,9 @@ for effector, limbId, comData, pointsData in zip(effectors,limbIds, compoints, p
# ~ hull_to_obj(hptsRF,points[0],"anymal_LF_constraints_in_RF.obj")
# ~ hull_to_obj(hptsLF,points[1],"anymal_RF_constraints_in_LF.obj")
-
# ~ for k in range(2):
- # ~ hcom = ConvexHull(compoints[k])
- # ~ plot_hull(hcom, compoints[k], array(compoints[k]))
-
- # ~ hpts = ConvexHull(points[k])
- # ~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)
+# ~ hcom = ConvexHull(compoints[k])
+# ~ plot_hull(hcom, compoints[k], array(compoints[k]))
+
+# ~ hpts = ConvexHull(points[k])
+# ~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)