Unverified Commit adba0d18 authored by Carlos Mastalli's avatar Carlos Mastalli Committed by GitHub
Browse files

Merge pull request #28 from cmastalli/topic/anymal-sim

Reverted back removation of base_inertia + added tiny inertia in base
parents 93aa0025 bc53ee5d
Pipeline #9730 passed with stage
in 1 minute and 23 seconds
......@@ -27,27 +27,24 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % subpath)
def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, referencePose='half_sitting'):
rmodel = robot.model
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
if has_rotor_parameters:
pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
pinocchio.loadRotorParameters(model, SRDF_PATH, verbose)
model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(model, SRDF_PATH, verbose)
q0 = pinocchio.neutral(model)
if referencePose is not None:
robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
return
q0 = model.referenceConfigurations[referencePose].copy()
return q0
def addFreeFlyerJointLimits(robot):
rmodel = robot.model
ub = rmodel.upperPositionLimit
def addFreeFlyerJointLimits(model):
ub = model.upperPositionLimit
ub[:7] = 1
rmodel.upperPositionLimit = ub
lb = rmodel.lowerPositionLimit
model.upperPositionLimit = ub
lb = model.lowerPositionLimit
lb[:7] = -1
rmodel.lowerPositionLimit = lb
model.lowerPositionLimit = lb
def loadANYmal(withArm=None):
......@@ -65,9 +62,9 @@ def loadANYmal(withArm=None):
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -81,7 +78,7 @@ def loadTalosArm():
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
return robot
......@@ -94,10 +91,10 @@ def loadTalos():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -155,11 +152,11 @@ def loadTalosLegs():
# Load SRDF file
robot.q0 = robot.q0[:robot.model.nq]
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
assert ((m2.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -172,9 +169,9 @@ def loadHyQ():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -190,9 +187,9 @@ def loadSolo(solo=True):
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -205,7 +202,7 @@ def loadKinova():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "arm_up")
return robot
......@@ -218,7 +215,7 @@ def loadTiago():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
# robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
return robot
......@@ -231,7 +228,7 @@ def loadTiagoNoHand():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
# robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
return robot
......@@ -247,9 +244,9 @@ def loadICub(reduced=True):
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
addFreeFlyerJointLimits(robot.model)
return robot
......@@ -258,12 +255,12 @@ def loadUR(robot=5, limited=False, gripper=False):
URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
model = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
if robot == 5 or robot == 3 and gripper:
SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
SRDF_SUBPATH = "/ur_description/srdf/" + SRDF_FILENAME
readParamsFromSrdf(model, modelPath + SRDF_SUBPATH, False, False, None)
return model
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, None)
return robot
def loadHector():
......@@ -288,9 +285,10 @@ def loadRomeo():
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
def loadIris():
URDF_FILENAME = "iris_simple.urdf"
URDF_SUBPATH = "/iris_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
return robot
\ No newline at end of file
return robot
......@@ -101,25 +101,24 @@
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
<mass value="16.793507758"/>
<inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
<mass value="1e-6"/>
<inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
</inertial>
</link>
<!-- Fixed joint to add dummy inertia link -->
<!-- <joint name="base_to_base_inertia" type="fixed">
<joint name="base_to_base_inertia" type="fixed">
<parent link="base"/>
<child link="base_inertia"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint> -->
</joint>
<!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
<!-- <link name="base_inertia">
<link name="base_inertia">
<inertial>
<origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
<mass value="16.793507758"/>
<inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
</inertial>
</link> -->
</link>
<!-- Xacro:Properties -->
<!-- [kg * m^2] -->
<!-- [A] -->
......
......@@ -101,25 +101,24 @@
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
<mass value="16.793507758"/>
<inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
<mass value="1e-6"/>
<inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
</inertial>
</link>
<!-- Fixed joint to add dummy inertia link -->
<!-- <joint name="base_to_base_inertia" type="fixed">
<joint name="base_to_base_inertia" type="fixed">
<parent link="base"/>
<child link="base_inertia"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint> -->
</joint>
<!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
<!-- <link name="base_inertia">
<link name="base_inertia">
<inertial>
<origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
<mass value="16.793507758"/>
<inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
</inertial>
</link> -->
</link>
<!-- Xacro:Properties -->
<!-- [kg * m^2] -->
<!-- [A] -->
......
<?xml version="1.0" ?>
<robot name="iCub">
<link name="base_link" />
<link name="base_link">
<inertial>
<mass value="1e-6"/>
<inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
</inertial>
</link>
<link name="chest">
<inertial>
<mass value="4.81" />
......
<?xml version="1.0" ?>
<robot name="iCub">
<link name="base_link" />
<link name="base_link">
<inertial>
<mass value="1e-6"/>
<inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
</inertial>
</link>
<link name="chest">
<inertial>
<mass value="4.81" />
......
Markdown is supported
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