Unverified Commit 7fe3792e authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub

Merge pull request #32 from jcarpent/master

Add Panda robot
parents ea2d6af2 6a180774
Pipeline #9878 passed with stage
in 4 minutes and 58 seconds
# flake8: noqa
from .robots_loader import (getModelPath, loadANYmal, loadHyQ, loadICub, loadKinova, loadRomeo, loadSolo, loadTalos,
loadTalosArm, loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf,
loadHector, loadDoublePendulum, loadIris)
from .robots_loader import (getModelPath, loadANYmal, loadDoublePendulum, loadHector, loadHyQ, loadICub, loadIris,
loadKinova, loadPanda, loadRomeo, loadSolo, loadTalos, loadTalosArm, loadTalosLegs,
loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf)
......@@ -8,7 +8,7 @@ eigenpy.switchToNumpyMatrix()
ROBOTS = [
'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova', 'tiago',
'tiago_no_hand', 'icub', 'ur5', 'romeo', 'hector', 'double_pendulum', 'iris'
'tiago_no_hand', 'icub', 'ur5', 'romeo', 'hector', 'double_pendulum', 'iris', 'panda'
]
parser = ArgumentParser()
......@@ -100,3 +100,8 @@ if args.robot == 'iris':
iris = robots_loader.loadIris()
iris.initViewer(loadModel=True)
iris.display(iris.q0)
if args.robot == 'panda':
panda = robots_loader.loadPanda()
panda.initViewer(loadModel=True)
panda.display(panda.q0)
......@@ -27,6 +27,10 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % subpath)
def getVisualPath(modelPath):
return join(modelPath, '../..')
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
if has_rotor_parameters:
pinocchio.loadRotorParameters(model, SRDF_PATH, verbose)
......@@ -60,7 +64,8 @@ def loadANYmal(withArm=None):
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())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
# Add the free-flyer joint limits
......@@ -75,7 +80,7 @@ def loadTalosArm():
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
......@@ -89,7 +94,8 @@ def loadTalos():
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
......@@ -167,7 +173,8 @@ def loadHyQ():
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
......@@ -185,7 +192,8 @@ def loadSolo(solo=True):
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "standing")
# Add the free-flyer joint limits
......@@ -200,7 +208,7 @@ def loadKinova():
URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, "arm_up")
return robot
......@@ -213,7 +221,7 @@ def loadTiago():
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
# Load SRDF file
# robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
return robot
......@@ -226,7 +234,7 @@ def loadTiagoNoHand():
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
# Load SRDF file
# robot.q0 = readParamsFromSrdf(robot.model, modelPath+SRDF_SUBPATH, False)
return robot
......@@ -242,7 +250,8 @@ def loadICub(reduced=True):
URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
# Load SRDF file
robot.q0 = readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False)
# Add the free-flyer joint limits
......@@ -250,12 +259,21 @@ def loadICub(reduced=True):
return robot
def loadPanda():
URDF_FILENAME = "panda.urdf"
URDF_SUBPATH = "/panda_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
return robot
def loadUR(robot=5, limited=False, gripper=False):
assert (not (gripper and (robot == 10 or limited)))
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)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(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
......@@ -267,7 +285,8 @@ def loadHector():
URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
return robot
......@@ -275,7 +294,7 @@ def loadDoublePendulum():
URDF_FILENAME = "double_pendulum.urdf"
URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)])
return robot
......@@ -283,12 +302,14 @@ def loadRomeo():
URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(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())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [getVisualPath(modelPath)],
pinocchio.JointModelFreeFlyer())
return robot
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
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" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from /opt/ros/kinetic/share/moveit_resources/panda_description/urdf/panda.urdf | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="panda_link0">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link0.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link0.stl" />
</geometry>
</collision>
</link>
<link name="panda_link1">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link1.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link1.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<origin rpy="0 0 0" xyz="0 0 0.333" />
<parent link="panda_link0" />
<child link="panda_link1" />
<axis xyz="0 0 1" />
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.3925" />
</joint>
<link name="panda_link2">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link2.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link2.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" />
<origin rpy="-1.57079632679 0 0" xyz="0 0 0" />
<parent link="panda_link1" />
<child link="panda_link2" />
<axis xyz="0 0 1" />
<limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.3925" />
</joint>
<link name="panda_link3">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link3.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link3.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0" />
<parent link="panda_link2" />
<child link="panda_link3" />
<axis xyz="0 0 1" />
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.3925" />
</joint>
<link name="panda_link4">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link4.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link4.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="0.0175" />
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0" />
<parent link="panda_link3" />
<child link="panda_link4" />
<axis xyz="0 0 1" />
<limit effort="87" lower="-3.1416" upper="0.0873" velocity="2.3925" />
</joint>
<link name="panda_link5">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link5.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link5.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0" />
<parent link="panda_link4" />
<child link="panda_link5" />
<axis xyz="0 0 1" />
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.8710" />
</joint>
<link name="panda_link6">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link6.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link6.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" />
<origin rpy="1.57079632679 0 0" xyz="0 0 0" />
<parent link="panda_link5" />
<child link="panda_link6" />
<axis xyz="0 0 1" />
<limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.8710" />
</joint>
<link name="panda_link7">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/link7.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/link7.stl" />
</geometry>
</collision>
</link>
<joint name="panda_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0" />
<parent link="panda_link6" />
<child link="panda_link7" />
<axis xyz="0 0 1" />
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.8710" />
</joint>
<link name="panda_link8" />
<joint name="panda_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107" />
<parent link="panda_link7" />
<child link="panda_link8" />
<axis xyz="0 0 0" />
</joint>
<joint name="panda_hand_joint" type="fixed">
<parent link="panda_link8" />
<child link="panda_hand" />
<origin rpy="0 0 -0.785398163397" xyz="0 0 0" />
</joint>
<link name="panda_hand">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/hand.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/hand.stl" />
</geometry>
</collision>
</link>
<link name="panda_leftfinger">
<visual>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/finger.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/finger.stl" />
</geometry>
</collision>
</link>
<link name="panda_rightfinger">
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0" />
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/visual/finger.dae" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0" />
<geometry>
<mesh filename="package://example-robot-data/robots/panda_description/meshes/collision/finger.stl" />
</geometry>
</collision>
</link>
<joint name="panda_finger_joint1" type="prismatic">
<parent link="panda_hand" />
<child link="panda_leftfinger" />
<origin rpy="0 0 0" xyz="0 0 0.0584" />
<axis xyz="0 1 0" />
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2" />
</joint>
<joint name="panda_finger_joint2" type="prismatic">
<parent link="panda_hand" />
<child link="panda_rightfinger" />
<origin rpy="0 0 0" xyz="0 0 0.0584" />
<axis xyz="0 -1 0" />
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2" />
<mimic joint="panda_finger_joint1" />
</joint>
</robot>
......@@ -125,11 +125,17 @@ class RomeoTest(RobotTestCase):
RobotTestCase.NV = 61
class PandaTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadPanda()
RobotTestCase.NQ = 9
RobotTestCase.NV = 9
if __name__ == '__main__':
test_classes_to_run = [
ANYmalTest, ANYmalKinovaTest, HyQTest, TalosTest, TalosArmTest, TalosArmFloatingTest, TalosLegsTest, ICubTest,
SoloTest, Solo12Test, TiagoTest, TiagoNoHandTest, UR5Test, UR5LimitedTest, UR5GripperTest, KinovaTest,
RomeoTest
RomeoTest, PandaTest
]
loader = unittest.TestLoader()
suites_list = []
......
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