Commit 37853ec6 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] use example-robot-data

parent 7483990d
......@@ -19,8 +19,10 @@ SET(${PROJECT_NAME}_PYTHON_UTILS
SET(${PROJECT_NAME}_PYTHON_TESTS
__init__.py
test_control_manager.py
robot_data_test.py
test_balance_ctrl_openhrp.py
test_control_manager.py
test_torque_offset_estimator.py
test_velocity_filters.py
)
......@@ -39,29 +41,3 @@ ENDFOREACH(file ${${PROJECT_NAME}_PYTHON_UTILS})
FOREACH(file ${${PROJECT_NAME}_PYTHON_TESTS})
PYTHON_INSTALL_ON_SITE("dynamic_graph/${PYTHON_DIR}/tests" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_PYTHON_TESTS})
SET(${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES)
IF(TALOS_DATA_FOUND)
SET(${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES
${${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES}
test_torque_offset_estimator
)
ENDIF(TALOS_DATA_FOUND)
IF(SIMPLE_HUMANOID_DESCRIPTION_FOUND)
SET(${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES
${${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES}
robot_data_test
)
ENDIF(SIMPLE_HUMANOID_DESCRIPTION_FOUND)
FOREACH(py_filename ${${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES})
CONFIGURE_FILE(
"dynamic_graph/${PYTHON_DIR}/tests/${py_filename}.py.in"
"dynamic_graph/${PYTHON_DIR}/tests/${py_filename}.py"
)
PYTHON_INSTALL_ON_SITE("dynamic_graph/${PYTHON_DIR}/tests" ${py_filename}.py)
ENDFOREACH(py_filename ${${PROJECT_NAME}_PYTHON_CONFIGURABLE_FILES})
import numpy as np
import example_robot_data
class initRobotData:
nbJoints = 29
testRobotPath = "/opt/openrobots/share/simple_humanoid_description/urdf/simple_humanoid.urdf"
controlDT = 0.005
maxCurrent = 5
robotRef = "control-manager-robot"
......@@ -87,6 +88,10 @@ class initRobotData:
RightFootSensorXYZ = (0.0, 0.0, -0.085)
def __init__(self):
_, _, urdf, _ = example_robot_data.load_full('simple_humanoid')
self.testRobotPath = urdf
def init_and_set_controller_manager(self, cm):
# Init should be called before addCtrlMode
# because the size of state vector must be known.
......
import numpy as np
class initRobotData:
nbJoints = 29
testRobotPath = "@SIMPLE_HUMANOID_DESCRIPTION_DATAROOTDIR@/simple_humanoid_description/urdf/simple_humanoid.urdf"
controlDT = 0.005
maxCurrent = 5
robotRef = "control-manager-robot"
urdftosot = (12, 13, 14, 15, 23, 24, 25, 26, 27, 28, 16, 17, 18, 19, 20, 21, 22, 6, 7, 8, 9, 10, 11, 0, 1, 2, 3, 4,
5)
ctrlManagerCurrentToControlGain = 1.0
mapJointNameToID = {
'rhy': 0,
'rhr': 1,
'rhp': 2,
'rk': 3,
'rap': 4,
'rar': 5,
'lhy': 6,
'lhr': 7,
'lhp': 8,
'lk': 9,
'lap': 10,
'lar': 11,
'ty': 12,
'tp': 13,
'hp': 14,
'rsp': 15,
'rsr': 16,
'rsy': 17,
're': 18,
'rwy': 19,
'rwp': 20,
'rh': 21,
'lsp': 22,
'lsr': 23,
'lsy': 24,
'le': 25,
'lwy': 26,
'lwp': 27,
'lh': 28
}
mapJointLimits = {
0: [-0.785398, 0.523599],
1: [-0.610865, 0.349066],
2: [-2.18166, 0.733038],
3: [-0.0349066, 2.61799],
4: [-1.309, 0.733038],
5: [-0.349066, 0.610865],
6: [-0.523599, 0.785398],
7: [-0.349066, 0.610865],
8: [-2.18166, 0.733038],
9: [-0.0349066, 2.61799],
10: [-1.309, 0.733038],
11: [-0.610865, 0.349066],
12: [-0.785398, 0.785398],
13: [-0.0872665, 1.0472],
14: [-0.785398, 0.78539],
15: [-0.523599, 0.785398],
16: [-3.14159, 1.0472],
17: [-1.65806, 0.174533],
18: [-1.6057, 1.6057],
19: [-2.3911, 0.0349066],
20: [-1.6057, 1.6057],
21: [-1.6057, 1.6057],
22: [-1.0, 1.0],
23: [-3.14159, 1.0472],
24: [-0.174533, 1.65806],
25: [-1.6057, 1.6057],
26: [-2.3911, 0.0349066],
27: [-1.6057, 1.6057],
28: [-1.6057, 1.6057],
}
fMax = np.array([100.0, 100.0, 300.0, 80.0, 80.0, 30.0])
fMin = -fMax
mapForceIdToForceLimits = {0: [fMin, fMax], 1: [fMin, fMax], 2: [fMin, fMax], 3: [fMin, fMax]}
mapNameToForceId = {"rf": 0, "lf": 1, "rh": 2, "lh": 3}
indexOfForceSensors = ()
FootFrameNames = {"Right": "RLEG_ANKLE_R", "Left": "LLEG_ANKLE_R"}
RightFootSensorXYZ = (0.0, 0.0, -0.085)
def init_and_set_controller_manager(self, cm):
# Init should be called before addCtrlMode
# because the size of state vector must be known.
cm.init(self.controlDT, self.testRobotPath, self.robotRef)
# Set the map from joint name to joint ID
for key in self.mapJointNameToID:
cm.setNameToId(key, self.mapJointNameToID[key])
# Set the map joint limits for each id
for key in self.mapJointLimits:
cm.setJointLimitsFromId(key, self.mapJointLimits[key][0], self.mapJointLimits[key][1])
# Set the force limits for each id
for key in self.mapForceIdToForceLimits:
cm.setForceLimitsFromId(key, np.array(self.mapForceIdToForceLimits[key][0]),
np.array(self.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in self.mapNameToForceId:
cm.setForceNameToForceId(key, self.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
cm.setJointsUrdfToSot(np.array(self.urdftosot))
# Set the foot frame name
for key in self.FootFrameNames:
cm.setFootFrameName(key, self.FootFrameNames[key])
cm.setRightFootSoleXYZ(np.array(self.RightFootSensorXYZ))
......@@ -4,25 +4,39 @@
@author: Rohan Budhiraja
"""
from __future__ import print_function
import numpy as np
from numpy import eye
import pinocchio as se3
from pinocchio import Motion, rnea
from pinocchio.robot_wrapper import RobotWrapper
import example_robot_data
from dynamic_graph import plug
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.torque_control.torque_offset_estimator import TorqueOffsetEstimator as TOE
# ______________________________________________________________________________
# ******************************************************************************
#
#1) The simplest robot task: Just go and reach a point
#2) While performing this task:
# ) find the torque values and feed them to the sensor calibration entity.
#3) Confirm that the offsets being calculated are correct.
# 1) The simplest robot task: Just go and reach a point
# 2) While performing this task:
# find the torque values and feed them to the sensor calibration entity.
# 3) Confirm that the offsets being calculated are correct.
# ______________________________________________________________________________
# ******************************************************************************
# Requires sot-dynamic-pinocchio
#-----------------------------------------------------------------------------
# -----------------------------------------------------------------------------
#SET THE PATH TO THE URDF AND MESHES
#Define robotName, urdfpath and initialConfig
dt = 5e-3
urdfPath = "@TALOS_DATA_DATAROOTDIR@/talos-data/robots/talos_reduced.urdf"
urdfDir = ["@TALOS_DATA_DATAROOTDIR@"]
robotName = 'TALOS'
OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
......@@ -62,7 +76,7 @@ initialConfig = (
0.,
0.1,
0.0, # Left Arm
#0., 0.,0.,0., 0.,0.,0., # Left gripper
# 0., 0.,0.,0., 0.,0.,0., # Left gripper
-0.261799,
-0.17453,
0.,
......@@ -71,34 +85,12 @@ initialConfig = (
0.,
0.1,
0.0, # Right Arm
#0., 0.,0.,0., 0.,0.,0., # Right gripper
# 0., 0.,0.,0., 0.,0.,0., # Right gripper
0.,
0. # Head
)
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
import numpy as np
#-----------------------------------------------------------------------------
import pinocchio as se3
from numpy import eye
from pinocchio import Motion, rnea
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core.matrix_util import matrixToTuple
# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
# Sensor Calibration Entity.
from dynamic_graph.sot.torque_control.torque_offset_estimator import TorqueOffsetEstimator as TOE
pinocchioRobot, _, urdfPath, _ = example_robot_data.load_full('talos', display=True)
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
......@@ -129,20 +121,23 @@ taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
#define contactLF and contactRF
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact' + name] = contact
# define contactLF and contactRF
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
targetRH = (0.5, -0.2, 1.0)
targetLH = (0.5, 0.2, 1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
# addRobotViewer(robot, small=False)
# robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9))
gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9))
......@@ -164,15 +159,15 @@ toe.gyroscope.value = (0., 0., 0.)
gravity = Motion.Zero()
gravity.linear = np.asarray((0.0, 0.0, -9.81))
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
robot.device.increment(dt)
np.set_printoptions(suppress=True)
def runner(n):
for i in xrange(n):
for i in range(n):
q_pin = fromSotToPinocchio(robot.device.state.value)
q_pin[0, :6] = 0.0
q_pin[0, 6] = 1.0
......@@ -192,7 +187,7 @@ toe.computeOffset(100, 1.0)
runner(100)
runner(1000)
print "The desired offset is", tau_offset
print "The obtained offset is", toe.getSensorOffsets()
print "The test is passed: ", (np.absolute(np.absolute(np.asarray(toe.getSensorOffsets())).max() - ref_offset) <
epsilon)
print("The desired offset is", tau_offset)
print("The obtained offset is", toe.getSensorOffsets())
print("The test is passed: ",
(np.absolute(np.absolute(np.asarray(toe.getSensorOffsets())).max() - ref_offset) < epsilon))
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