Commit 77b0ade3 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'devel' into release/1.3.0

parents e7a8ca41 7632e95d
......@@ -28,7 +28,8 @@ def makeRobot():
DeviceTalos = PyEntityFactoryClass('DeviceTalos')
# Create the robot using the device.
robot = Robot(name='talos', device=DeviceTalos('PYRENE'), fromRosParam=True)
robot = Robot(name='talos', device=DeviceTalos('PYRENE'),
fromRosParam=True)
robot.dynamic.com.recompute(0)
_com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.)
......
......@@ -9,46 +9,58 @@ class Robot(Talos):
This class instantiates LAAS TALOS Robot
"""
def defineHalfSitting (self, q):
def defineHalfSitting(self, q):
"""
q is the configuration to fill.
When this function is called, the attribute pinocchioModel has been filled.
When this function is called,
the attribute pinocchioModel has been filled.
"""
model = self.pinocchioModel
# Free flyer
q[2] = 1.018213
# left leg
self.setJointValueInConfig(q,
[ "leg_left_{}_joint".format(i+1) for i in range(6) ],
[ 0., 0., -0.411354, 0.859395, -0.448041, -0.001708,])
["leg_left_{}_joint".format(i+1)
for i in range(6)],
[0., 0., -0.411354, 0.859395,
-0.448041, -0.001708])
# right leg
self.setJointValueInConfig(q,
[ "leg_right_{}_joint".format(i+1) for i in range(6) ],
[ 0., 0., -0.411354, 0.859395, -0.448041, -0.001708,])
["leg_right_{}_joint".format(i+1)
for i in range(6)],
[0., 0., -0.411354, 0.859395,
-0.448041, -0.001708])
# torso
self.setJointValueInConfig(q,
[ "torso_{}_joint".format(i+1) for i in range(2) ],
[ 0., 0.006761])
["torso_{}_joint".format(i+1)
for i in range(2)],
[0., 0.006761])
# left arm
self.setJointValueInConfig(q,
[ "arm_left_{}_joint".format(i+1) for i in range(7) ],
[ 0.25847, 0.173046, -0.0002, -0.525366, 0., -0., 0.1 ])
["arm_left_{}_joint".format(i+1)
for i in range(7)],
[0.25847, 0.173046, -0.0002,
-0.525366, 0., -0., 0.1])
# right arm
self.setJointValueInConfig(q,
[ "arm_right_{}_joint".format(i+1) for i in range(7) ],
[ -0.25847, -0.173046, 0.0002, -0.525366, 0., 0., 0.1 ])
["arm_right_{}_joint".format(i+1)
for i in range(7)],
[-0.25847, -0.173046, 0.0002,
-0.525366, 0., 0., 0.1])
# grippers
self.setJointValueInConfig(q,
[ "gripper_left_joint", "gripper_right_joint" ],
[ -0.005, -0.005 ])
["gripper_left_joint",
"gripper_right_joint"],
[-0.005, -0.005])
# torso
self.setJointValueInConfig(q,
[ "head_{}_joint".format(i+1) for i in range(2) ],
[ 0., 0.])
["head_{}_joint".format(i+1)
for i in range(2)],
[0., 0.])
def __init__(self, name, device=None, tracer=None, fromRosParam=None):
Talos.__init__(self, name, device=device, tracer=tracer, fromRosParam=fromRosParam)
Talos.__init__(self, name, device=device, tracer=tracer,
fromRosParam=fromRosParam)
"""
TODO:Confirm these values
# Define camera positions w.r.t gaze.
......
......@@ -26,7 +26,8 @@ def convert(filename):
openhrpZmp = np.genfromtxt(filename + '.zmp')
nbConfig = len(openhrpPos)
if len(openhrpZmp) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.")
raise RuntimeError(filename + ".pos and " + filename +
".zmp have different lengths.")
try:
openhrpHip = np.genfromtxt(filename + '.hip')
except IOError:
......@@ -41,7 +42,8 @@ def convert(filename):
openhrpHip = np.array(hip)
if len(openhrpHip) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".hip have different lengths.")
raise RuntimeError(filename + ".pos and " + filename +
".hip have different lengths.")
t = 1
featurePos = []
......@@ -114,8 +116,10 @@ def convert(filename):
0.,
))
t += 1
fixedLeftFoot = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
fixedRightFoot = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
fixedLeftFoot = SE3(robot.leftAnkle.position.value) \
* R3(0., 0., -0.107)
fixedRightFoot = SE3(robot.rightAnkle.position.value) \
* R3(0., 0., -0.107)
filePos = open(filename + '.posture', 'w')
fileLa = open(filename + '.la', 'w')
......@@ -125,8 +129,9 @@ def convert(filename):
fileFr = open(filename + '.fr', 'w')
dt = .005
for (pos, la, ra, com, force_lf, force_rf, i) in zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
forceRightFoot, range(10000000)):
for (pos, la, ra, com, force_lf, force_rf, i) in \
zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
forceRightFoot, range(10000000)):
t = i * dt
filePos.write("{0}".format(t))
fileLa.write("{0}".format(t))
......
......@@ -5,8 +5,6 @@ from __future__ import print_function
import pinocchio
from dynamic_graph import plug
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import \
AbstractHumanoidRobot
......@@ -27,8 +25,10 @@ class Talos(AbstractHumanoidRobot):
This class defines a Talos robot
"""
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
defaultFilename = "package://talos_data/urdf/talos_reduced_v2.urdf"
"""
TODO: Confirm the position and existence of these sensors
......@@ -50,7 +50,8 @@ class Talos(AbstractHumanoidRobot):
# Gripper position in full configuration: 27:34, and 41:48
# Small configuration: 36 DOF
# Full configuration: 50 DOF
res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
res = config[0:27] + 7 * (0., ) + config[27:34] \
+ 7 * (0., ) + config[34:]
return res
def __init__(self, name, device=None, tracer=None, fromRosParam=False):
......@@ -73,9 +74,47 @@ class Talos(AbstractHumanoidRobot):
paramName = "/robot_description"
self.param_server = ParameterServer("param_server")
self.param_server.init_simple(ltimeStep)
model2_string=self.param_server.getParameter(paramName)
self.param_server.setParameter("/pg/remap/l_ankle",
"leg_left_6_link")
self.param_server.setParameter("/pg/remap/r_ankle",
"leg_right_6_link")
self.param_server.setParameter("/pg/remap/l_wrist",
"arm_left_7_link")
self.param_server.setParameter("/pg/remap/r_wrist",
"arm_right_7_link")
self.param_server.setParameter("/pg/remap/body",
"base_link")
self.param_server.setParameter("/pg/remap/torso",
"torso_2_link")
lpn_pre = "/robot/specificities/feet/"
feet = ['right', 'left']
for afoot in feet:
self.param_server.setParameterDbl(lpn_pre + afoot +
"/size/height",
0.122)
self.param_server.setParameterDbl(lpn_pre + afoot +
"/size/width",
0.205)
self.param_server.setParameterDbl(lpn_pre + afoot +
"/size/depth",
0.107)
self.param_server.setParameterDbl(lpn_pre + afoot +
"/anklePosition/x",
0.0)
self.param_server.setParameterDbl(lpn_pre + afoot +
"/anklePosition/y",
0.0)
self.param_server.setParameterDbl(lpn_pre + afoot +
"/anklePosition/z",
0.107)
self.param_server.displayRobotUtil()
model2_string = self.param_server.getParameter(paramName)
self.loadModelFromString(model2_string,
rootJointType=pinocchio.JointModelFreeFlyer,
rootJointType=pinocchio.
JointModelFreeFlyer,
removeMimicJoints=True)
else:
self.loadModelFromUrdf(self.defaultFilename,
......@@ -102,11 +141,14 @@ class Talos(AbstractHumanoidRobot):
self.initializeRobot()
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle, self.OperationalPointsMap["left-ankle"]))
("leftFootForceSensor", self.forceSensorInLeftAnkle,
self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle, self.OperationalPointsMap["right-ankle"]))
("rightFootForceSensor", self.forceSensorInRightAnkle,
self.OperationalPointsMap["right-ankle"]))
# Create operational points based on operational points map (if provided)
# Create operational points based on operational points map
# (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
......
......@@ -100,6 +100,8 @@ if __name__ == '__main__':
state = client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state)))
rospy.loginfo("Action finished succesfully with state: "
+ str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
rospy.logwarn("Action failed with state: "
+ str(get_status_string(state)))
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