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

Merge branch 'topic/kinova' into 'devel'

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

See merge request !18
parents 5af2f8de 6286dd38
Pipeline #6553 passed with stage
in 8 minutes and 45 seconds
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