Commit 7d4bf844 authored by Carlos Mastalli's avatar Carlos Mastalli

Merge branch 'topic/kinova' into 'devel'

Added the Kinova arm + ANYmal with kinova; renamed the reference postures

See merge request gepetto/example-robot-data!18
parents 5af2f8de 6286dd38
*.pyc
*.pyd
*.pyo
build*
......@@ -43,6 +43,7 @@ IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(DIRECTORY solo_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY icub_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY talos_data DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY kinova_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY tiago_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY ur_description DESTINATION share/${PROJECT_NAME})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
......
......@@ -46,7 +46,7 @@ If you have never added robotpkg as a softwares repository, please follow first
(you will need pinocchio and its Python bindings)
`python -m example_robot_data [anymal,hyq,solo,solo12,talos,talos_arm,talos_legs,tiago,tiago_no_hand,icub,ur5]`
`python -m example_robot_data [anymal,anymal_kinova,hyq,solo,solo12,talos,talos_arm,talos_legs,tiago,tiago_no_hand,icub,ur5]`
This will work from the `python` subdirectory inside this repository, or if this package has been installed on your
system.
This diff is collapsed.
<?xml version="1.0" ?>
<robot name="anymal_kinova">
<group name="lf_leg">
<joint name="LF_HAA" />
<joint name="LF_HFE" />
<joint name="LF_KFE" />
<chain base_link="base" tip_link="LF_FOOT" />
</group>
<group name="lh_leg">
<joint name="LH_HAA" />
<joint name="LH_HFE" />
<joint name="LH_KFE" />
<chain base_link="base" tip_link="LH_FOOT" />
</group>
<group name="rf_leg">
<joint name="RF_HAA" />
<joint name="RF_HFE" />
<joint name="RF_KFE" />
<chain base_link="base" tip_link="RF_FOOT" />
</group>
<group name="rh_leg">
<joint name="RH_HAA" />
<joint name="RH_HFE" />
<joint name="RH_KFE" />
<chain base_link="base" tip_link="RH_FOOT" />
</group>
<group name="arm">
<joint name="j2s6s200_joint_1" />
<joint name="j2s6s200_joint_2" />
<joint name="j2s6s200_joint_3" />
<joint name="j2s6s200_joint_4" />
<joint name="j2s6s200_joint_5" />
<joint name="j2s6s200_joint_5" />
<chain base_link="base" tip_link="j2s6s200_end_effector" />
</group>
<group name="all_legs">
<group name="lf" />
<group name="rf" />
<group name="lh" />
<group name="rh" />
</group>
<group name="all">
<group name="lf" />
<group name="rf" />
<group name="lh" />
<group name="rh" />
<group name="arm" />
</group>
<group name="r_legs">
<group name="rf" />
<group name="rh" />
</group>
<group name="l_legs">
<group name="lf" />
<group name="lh" />
</group>
<group name="f_legs">
<group name="lf" />
<group name="rf" />
</group>
<group name="h_legs">
<group name="lh" />
<group name="rh" />
</group>
<group name="ld_legs">
<group name="lf" />
<group name="rh" />
</group>
<group name="rd_legs">
<group name="rf" />
<group name="lh" />
</group>
<end_effector name="lf_foot" parent_link="LF_FOOT" group="lf_leg" />
<end_effector name="rf_foot" parent_link="RF_FOOT" group="rf_leg" />
<end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" />
<end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
<end_effector name="arm" parent_link="j2s6s200_end_effector" group="arm" />
<group_state name="standing_with_arm_up" group="all">
<joint name="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
<joint name="LF_HAA" value="-0.1" />
<joint name="LF_HFE" value="0.7" />
<joint name="LF_KFE" value="-1." />
<joint name="RF_HAA" value="0.1" />
<joint name="RF_HFE" value="0.7" />
<joint name="RF_KFE" value="-1." />
<joint name="LH_HAA" value="-0.1" />
<joint name="LH_HFE" value="-0.7" />
<joint name="LH_KFE" value="1." />
<joint name="RH_HAA" value="0.1" />
<joint name="RH_HFE" value="-0.7" />
<joint name="RH_KFE" value="1." />
<joint name="j2s6s200_joint_1" value="1.5707" />
<joint name="j2s6s200_joint_2" value="2.618" />
<joint name="j2s6s200_joint_3" value="-1.5707" />
<joint name="j2s6s200_joint_4" value="3.1415" />
<joint name="j2s6s200_joint_5" value="2.618" />
<joint name="j2s6s200_joint_6" value="0." />
</group_state>
<disable_collisions link1="LF_HIP" link2="LF_THIGH" reason="Adjacent" />
<disable_collisions link1="LF_HIP" link2="LF_SHANK" reason="Never" />
<disable_collisions link1="LF_HIP" link2="LF_FOOT" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RF_HIP" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RF_THIGH" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RF_SHANK" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RF_FOOT" reason="Never" />
<disable_collisions link1="LF_HIP" link2="LH_HIP" reason="Never" />
<disable_collisions link1="LF_HIP" link2="LH_THIGH" reason="Never" />
<disable_collisions link1="LF_HIP" link2="LH_SHANK" reason="Never" />
<disable_collisions link1="LF_HIP" link2="LH_FOOT" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RH_HIP" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RH_THIGH" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RH_SHANK" reason="Never" />
<disable_collisions link1="LF_HIP" link2="RH_FOOT" reason="Never" />
<disable_collisions link1="base" link2="LF_HIP" reason="Adjacent" />
<disable_collisions link1="base" link2="LH_HIP" reason="Adjacent" />
<disable_collisions link1="base" link2="RF_HIP" reason="Adjacent" />
<disable_collisions link1="base" link2="RH_HIP" reason="Adjacent" />
<disable_collisions link1="LF_HIP" link2="LF_THIGH" reason="Adjacent" />
<disable_collisions link1="LH_HIP" link2="LH_THIGH" reason="Adjacent" />
<disable_collisions link1="RF_HIP" link2="RF_THIGH" reason="Adjacent" />
<disable_collisions link1="RH_HIP" link2="RH_THIGH" reason="Adjacent" />
<disable_collisions link1="LF_THIGH" link2="LF_SHANK" reason="Adjacent" />
<disable_collisions link1="LH_THIGH" link2="LH_SHANK" reason="Adjacent" />
<disable_collisions link1="RF_THIGH" link2="RF_SHANK" reason="Adjacent" />
<disable_collisions link1="RH_THIGH" link2="RH_SHANK" reason="Adjacent" />
<disable_collisions link1="LF_SHANK" link2="LF_FOOT" reason="Adjacent" />
<disable_collisions link1="LH_SHANK" link2="LH_FOOT" reason="Adjacent" />
<disable_collisions link1="RF_SHANK" link2="RF_FOOT" reason="Adjacent" />
<disable_collisions link1="RH_SHANK" link2="RH_FOOT" reason="Adjacent" />
<disable_collisions link1="LF_ADAPTER" link2="LF_FOOT" reason="Adjacent" />
<disable_collisions link1="LH_ADAPTER" link2="LH_FOOT" reason="Adjacent" />
<disable_collisions link1="RF_ADAPTER" link2="RF_FOOT" reason="Adjacent" />
<disable_collisions link1="RH_ADAPTER" link2="RH_FOOT" reason="Adjacent" />
<disable_collisions link1="LF_ADAPTER" link2="LF_SHANK" reason="Adjacent" />
<disable_collisions link1="LH_ADAPTER" link2="LH_SHANK" reason="Adjacent" />
<disable_collisions link1="RF_ADAPTER" link2="RF_SHANK" reason="Adjacent" />
<disable_collisions link1="RH_ADAPTER" link2="RH_SHANK" reason="Adjacent" />
<disable_collisions link1="base" link2="imu_link" reason="Adjacent" />
<!-- Need to check these 4 ? there may be auto-collisions ... -->
<disable_collisions link1="base" link2="LF_THIGH" reason="Adjacent" />
<disable_collisions link1="base" link2="LH_THIGH" reason="Adjacent" />
<disable_collisions link1="base" link2="RF_THIGH" reason="Adjacent" />
<disable_collisions link1="base" link2="RH_THIGH" reason="Adjacent" />
</robot>
......@@ -61,7 +61,7 @@
<end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" />
<end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
<group_state name="half_sitting" group="all_legs">
<group_state name="standing" group="all_legs">
<joint name="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
<joint name="LF_HAA" value="-0.1" />
<joint name="LF_HFE" value="0.7" />
......
......@@ -65,7 +65,7 @@
<end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" />
<end_effector name="rh_foot" parent_link="rh_foot" group="rh_leg" />
<group_state name="half_sitting" group="all_legs">
<group_state name="standing" group="all_legs">
<joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
<joint name="lf_haa_joint" value="-0.2" />
<joint name="lf_hfe_joint" value="0.75" />
......@@ -81,7 +81,7 @@
<joint name="rh_kfe_joint" value="1.5" />
</group_state>
<group_state name="standing" group="all_legs">
<group_state name="straight_standing" group="all_legs">
<joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
<joint name="lf_haa_joint" value="0." />
<joint name="lf_hfe_joint" value="0.75" />
......
This diff is collapsed.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
<?xml version="1.0" ?>
<robot name="kinova">
<group name="end_effector">
<joint name="j2s6s200_joint_1" />
<joint name="j2s6s200_joint_2" />
<joint name="j2s6s200_joint_3" />
<joint name="j2s6s200_joint_4" />
<joint name="j2s6s200_joint_5" />
<joint name="j2s6s200_joint_5" />
<chain base_link="base" tip_link="j2s6s200_end_effector" />
</group>
<end_effector name="end_effector" parent_link="j2s6s200_end_effector" group="end_effector" />
<group_state name="arm_up">
<joint name="j2s6s200_joint_1" value="1.5707" />
<joint name="j2s6s200_joint_2" value="2.618" />
<joint name="j2s6s200_joint_3" value="-1.5707" />
<joint name="j2s6s200_joint_4" value="3.1415" />
<joint name="j2s6s200_joint_5" value="2.618" />
<joint name="j2s6s200_joint_6" value="0." />
</group_state>
</robot>
# flake8: noqa
from .robots_loader import (getModelPath, loadANYmal, loadHyQ, loadICub, loadSolo, loadTalos, loadTalosArm,
loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf)
loadTalosLegs, loadKinova, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf)
......@@ -3,7 +3,8 @@ from argparse import ArgumentParser
from . import robots_loader
ROBOTS = [
'anymal', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'tiago', 'tiago_no_hand', 'icub', 'ur5'
'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova', 'tiago',
'tiago_no_hand', 'icub', 'ur5'
]
parser = ArgumentParser()
......@@ -16,17 +17,22 @@ if args.robot == 'anymal':
anymal.initViewer(loadModel=True)
anymal.display(anymal.q0)
if args.robot == 'hyq':
elif args.robot == 'anymal_kinova':
anymal = robots_loader.loadANYmal(withArm='kinova')
anymal.initViewer(loadModel=True)
anymal.display(anymal.q0)
elif args.robot == 'hyq':
hyq = robots_loader.loadHyQ()
hyq.initViewer(loadModel=True)
hyq.display(hyq.q0)
if args.robot == 'solo':
elif args.robot == 'solo':
solo = robots_loader.loadSolo()
solo.initViewer(loadModel=True)
solo.display(solo.q0)
if args.robot == 'solo12':
elif args.robot == 'solo12':
solo = robots_loader.loadSolo(False)
solo.initViewer(loadModel=True)
solo.display(solo.q0)
......@@ -41,11 +47,16 @@ elif args.robot == 'talos_arm':
talos_arm.initViewer(loadModel=True)
talos_arm.display(talos_arm.q0)
if args.robot == 'talos_legs':
elif args.robot == 'talos_legs':
talos_legs = robots_loader.loadTalosLegs()
talos_legs.initViewer(loadModel=True)
talos_legs.display(talos_legs.q0)
elif args.robot == 'kinova':
kinova = robots_loader.loadKinova()
kinova.initViewer(loadModel=True)
kinova.display(kinova.q0)
elif args.robot == 'tiago':
tiago = robots_loader.loadTiago()
tiago.initViewer(loadModel=True)
......
......@@ -8,13 +8,8 @@ from pinocchio.robot_wrapper import RobotWrapper
def getModelPath(subpath, printmsg=False):
base = '../../../share/example-robot-data'
for p in sys.path:
path = join(p, base.strip('/'))
if exists(join(path, subpath.strip('/'))):
if printmsg:
print("using %s as modelPath" % path)
return path
for path in (dirname(dirname(dirname(dirname(__file__)))), dirname(dirname(dirname(__file__)))):
for path in [dirname(dirname(dirname(dirname(__file__)))),
dirname(dirname(dirname(__file__)))] + [join(p, base.strip('/')) for p in sys.path]:
if exists(join(path, subpath.strip('/'))):
if printmsg:
print("using %s as modelPath" % path)
......@@ -22,14 +17,14 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % (subpath))
def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True):
def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, referencePose='half_sitting'):
rmodel = robot.model
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)
robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
return
......@@ -44,16 +39,22 @@ def addFreeFlyerJointLimits(robot):
rmodel.lowerPositionLimit = lb
def loadANYmal():
URDF_FILENAME = "anymal.urdf"
def loadANYmal(withArm=None):
if withArm is None:
URDF_FILENAME = "anymal.urdf"
SRDF_FILENAME = "anymal.srdf"
REF_POSTURE = "standing"
elif withArm == "kinova":
URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = "standing_with_arm_up"
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
SRDF_FILENAME = "anymal.srdf"
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
......@@ -154,7 +155,7 @@ def loadHyQ():
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
......@@ -172,12 +173,25 @@ 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)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadKinova():
URDF_FILENAME = "kinova.urdf"
SRDF_FILENAME = "kinova.srdf"
SRDF_SUBPATH = "/kinova_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
return robot
def loadTiago():
URDF_FILENAME = "tiago.urdf"
# SRDF_FILENAME = "tiago.srdf"
......@@ -223,7 +237,7 @@ def loadICub(reduced=True):
def loadUR(robot=5, limited=False):
URDF_FILENAME = 'ur%i%s_robot.urdf' % (robot, '_joint_limited' if limited else '')
URDF_SUBPATH = '/ur_description/urdf/' + URDF_FILENAME
URDF_FILENAME = "ur%i%s_robot.urdf" % (robot, "_joint_limited" if limited else '')
URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
......@@ -65,7 +65,7 @@
<end_effector name="lh_foot" parent_link="HL_FOOT" group="lh_leg" />
<end_effector name="rh_foot" parent_link="HR_FOOT" group="rh_leg" />
<group_state name="half_sitting" group="all_legs">
<group_state name="standing" group="all_legs">
<joint name="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
<joint name="FL_HAA" value="0.1" />
<joint name="FL_HFE" value="0.8" />
......@@ -81,7 +81,7 @@
<joint name="HR_KFE" value="1.6" />
</group_state>
<group_state name="standing" group="all_legs">
<group_state name="straight_standing" group="all_legs">
<joint name="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
<joint name="FL_HAA" value="0." />
<joint name="FL_HFE" value="0.8" />
......
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