def initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
def __init__(self, compositeName, robotName, load=True):
Parent.__init__(self, compositeName, robotName, self.rootJointType,
load)
self.rightAnkle = robotName + "/leg_right_6_joint"
self.leftAnkle = robotName + "/leg_left_6_joint"
self.rightWrist = robotName + "/arm_right_7_joint"
self.leftWrist = robotName + "/arm_left_7_joint"
def __init__ (self, compositeName, robotName, load = True,
rootJointType = "planar"):
Parent.__init__ (self, compositeName, robotName, rootJointType, load)
self.tf_root = "base_footprint"
self.halfSitting = self.getCurrentConfig ()
for j, v in self.halfSittingDict.iteritems ():
index = self.rankInConfiguration [robotName + "/" + j]
self.halfSitting [index] = v
def __init__(self,
compositeName,
robotName,
load=True,
rootJointType="planar",
**kwargs):
Parent.__init__(self, compositeName, robotName, rootJointType, load,
**kwargs)
def __init__(self,
compositeName,
robotName,
load=True,
rootJointType="anchor",
**kwargs):
Parent.__init__(self, compositeName, robotName, rootJointType, load,
**kwargs)
self.tf_root = "base_footprint"
def __init__(self,
compositeName,
robotName,
load=True,
rootJointType=rootJointType):
Parent.__init__(self, compositeName, robotName, rootJointType, load)
self.rightWrist = "wrist_3_joint"
self.leftWrist = "wrist_3_joint"
self.endEffector = "ee_fixed_joint"
def __init__ (self, compositeName, robotName):
Parent.__init__ (self, compositeName, robotName, self.rootJointType)
hs = self.halfSitting.copy ()
for joint, value in hs.iteritems ():
self.halfSitting [self.displayName + "/" + joint] = value
self.rightWrist = self.displayName + "/RARM_JOINT5"
self.leftWrist = self.displayName + "/LARM_JOINT5"
self.rightAnkle = self.displayName + "/RLEG_JOINT5"
self.leftAnkle = self.displayName + "/LLEG_JOINT5"
self.waist = self.displayName + "/base_joint_SO3"
self.chest = self.displayName + "/CHEST_JOINT1"
self.gaze = self.displayName + "/HEAD_JOINT1"
def makeRobotProblemAndViewerFactory(corbaclient):
robot = Robot("dev",
"talos",
rootJointType="freeflyer",
client=corbaclient)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
vf.loadObjectModel(Object, "box")
robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2])
vf.loadEnvironmentModel(Table, "table")
return robot, ps, vf
def __init__(
self, compositeName, robotName, load=True, rootJointType="planar", **kwargs
):
"""
Constructor
\\param compositeName name of the composite robot that will be built later,
\\param robotName name of the first robot that is loaded now,
\\param load whether to actually load urdf files. Set to no if you only
want to initialize a corba client to an already initialized
problem.
\\param rootJointType type of root joint among ("freeflyer", "planar",
"anchor"),
"""
Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs)
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
class Mire(object):
rootJointType = "freeflyer"
packageName = "agimus_demos"
urdfName = "calibration_mire"
urdfSuffix = ""
srdfSuffix = ""
name = "mire"
handles = ["mire/left", "mire/right"]
robot = Robot("dev", "talos", rootJointType="freeflyer")
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
vf.loadObjectModel(Mire, "mire")
Robot.meshPackageName = 'ur_description'
Robot.urdfName = 'ur5'
Robot.urdfSuffix = '_robot'
Robot.srdfSuffix = ''
class Box(object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('ur5-box', 'ur5', rootJointType='anchor')
ps = ProblemSolver(robot)
fk = ViewerFactory(ps)
fk.loadObjectModel(Box, 'box')
fk.buildCompositeRobot(['ur5', 'box'])
robot.setJointBounds("box/base_joint_xyz", [-2, +2, -2, +2, -2, 2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
rank = robot.rankInConfiguration['ur5/shoulder_lift_joint']
q_init[rank] = 0
rank = robot.rankInConfiguration['ur5/elbow_joint']
q_init[rank] = -PI / 2
rank = robot.rankInConfiguration['box/base_joint_xyz']
# /
import numpy as np
from hpp import Transform
from hpp.corbaserver.manipulation import ConstraintGraph, ProblemSolver, newProblem
from hpp.corbaserver.manipulation.robot import Robot
from hpp.gepetto.manipulation import ViewerFactory
newProblem()
Robot.packageName = "talos_data"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_full"
Robot.srdfSuffix = ""
robot = Robot("dev", "talos", rootJointType="freeflyer")
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
half_sitting = [
0,
class Calibration(object):
def __init__(self, client):
self.client = client
self.robot = None
self.robots = list()
self.robot_locks = list()
self.robots_locks = list()
self.robots_gaze = list()
self.robots_identity_constraints = list()
self.q = []
def initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
def setLockJoints(self, robot_id, joints_name_value_tuple):
self.ps.createLockedJoint(
"talos_" + str(robot_id) + "/root_joint",
"talos_" + str(robot_id) + "/root_joint",
[0, 0, 1, 0, 0, 0, 1],
)
root_joint_rank = self.robot.rankInConfiguration[
"talos_" + str(robot_id) + "/root_joint"
]
self.q[root_joint_rank : root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
self.robots_locks.append("talos_" + str(robot_id) + "/root_joint")
# self.ps.createLockedJoint("talos_" + str(robot_id) + "/calib_mire_joint_2", "talos_" + str(robot_id) + "/calib_mire_joint_2", [0, 0, 0, 0, 0, 0, 1])
# mire_joint_rank = self.robot.rankInConfiguration["talos_" + str(robot_id) + "/calib_mire_joint_2"]
# self.q[mire_joint_rank:mire_joint_rank + 7] = [0, 0, 0, 0, 0, 0, 1]
# self.robots_locks.append("talos_" + str(robot_id) + "/calib_mire_joint_2")
for name, value in joints_name_value_tuple:
joint_name = "talos_" + str(robot_id) + "/" + name
self.q[self.robot.rankInConfiguration[joint_name]] = value
self.ps.createLockedJoint(joint_name, joint_name, [value])
self.robots_locks.append(joint_name)
def getRankAndConfigSize(self, robot_id):
robot_name = "talos_" + str(robot_id)
rank = self.robot.rankInConfiguration[robot_name + "/root_joint"]
size = sum(
[
self.robot.getJointConfigSize(joint_name)
for joint_name in calib.robot.getJointNames()
if robot_name in joint_name
]
)
return rank, size
def setGaze(self, robot_id, checkerboard_pose):
robot_name = "talos_" + str(robot_id)
position = (
checkerboard_pose.position.x,
checkerboard_pose.position.y,
checkerboard_pose.position.z,
)
orientation = (
checkerboard_pose.orientation.x,
checkerboard_pose.orientation.y,
checkerboard_pose.orientation.z,
checkerboard_pose.orientation.w,
)
self.ps.createPositionConstraint(
robot_name + "gaze",
robot_name + "/calib_rgb_joint",
robot_name + "/calib_mire_joint_2",
position,
(0, 0, 0),
[True, True, True],
)
self.ps.createOrientationConstraint(
robot_name + "gaze_O",
robot_name + "/calib_mire_joint_2",
robot_name + "/calib_rgb_joint",
orientation,
[True, True, True],
)
self.robots_gaze.append(robot_name + "gaze")
self.robots_gaze.append(robot_name + "gaze_O")
def constrainFreeflyers(self):
for robot_id in range(1, len(self.robots)):
robot_name = "talos_" + str(robot_id)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_rgb",
["talos_0/calib_rgb_joint"],
[robot_name + "/calib_rgb_joint"],
)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_mire",
["talos_0/calib_mire_joint_2"],
[robot_name + "/calib_mire_joint_2"],
)
self.robots_identity_constraints.append(robot_name + "_id_rgb")
self.robots_identity_constraints.append(robot_name + "_id_mire")
def optimize(self, q_init, robots_id):
self.q_proj = self.q
client.basic.problem.resetConstraints()
calib.robot.setCurrentConfig(q_init)
client.basic.problem.addLockedJointConstraints("unused1", calib.robots_locks)
gaze = [
c
for c in calib.robots_gaze
if any(["talos_" + str(robot_id) in c for robot_id in robots_id])
]
num_constraints = gaze + calib.robots_identity_constraints
client.basic.problem.addNumericalConstraints(
"unused2", num_constraints, [0 for _ in num_constraints]
)
# client.basic.problem.addNumericalConstraints("mighty_cam_cost", ["cam_cost"], [ 1 ])
# calib.client.basic.problem.setNumericalConstraintsLastPriorityOptional(True)
projOk, self.q_proj, error = client.basic.problem.applyConstraints(q_init)
if error < 1.0:
optOk, self.q_proj, error = client.basic.problem.optimize(self.q_proj)
if optOk:
print("All was good! " + str(max(error)))
else:
print("Optimisation error: " + str(max(error)))
else:
print("Projection error: " + str(error))
rank_rgb = calib.robot.rankInConfiguration["talos_0/calib_rgb_joint"]
rank_mire = calib.robot.rankInConfiguration["talos_0/calib_mire_joint_2"]
return (
self.q_proj,
self.q_proj[rank_rgb : rank_rgb + 7],
max(error),
self.q_proj[rank_mire : rank_mire + 7],
)
def resetConnection(self):
self.r = Robot("", "", "", False)
Robot.packageName = "talos_data"
Robot.urdfName = "talos"
Robot.urdfSuffix = '_full'
Robot.srdfSuffix= ''
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
srdfSuffix = ""
handles = [ "box/top", "box/bottom" ]
robot = Robot ('dev', 'talos', rootJointType = "freeflyer")
robot. leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver (robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory (ps)
Object = Box
def __init__ (self, compositeName, robotName, load = True,
rootJointType = rootJointType):
Parent.__init__ (self, compositeName, robotName, rootJointType, load)
self.rightWrist = "wrist_3_joint"
self.leftWrist = "wrist_3_joint"
self.endEffector = "ee_fixed_joint"
Robot.packageName = 'ur_description'
Robot.meshPackageName = 'ur_description'
Robot.urdfName = 'ur5'
Robot.urdfSuffix = '_robot'
Robot.srdfSuffix = ''
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('ur5-box', 'ur5', rootJointType = 'anchor')
ps = ProblemSolver (robot)
fk = ViewerFactory (ps)
fk.loadObjectModel (Box, 'box')
fk.buildCompositeRobot (['ur5', 'box'])
robot.setJointBounds ("box/base_joint_xyz", [-2,+2,-2,+2,-2,2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
q_init [rank] = 0
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_init [rank] = - PI / 2
rank = robot.rankInConfiguration ['box/base_joint_xyz']
t.translation.x = transform.translation[0]
t.translation.y = transform.translation[1]
t.translation.z = transform.translation[2]
publisher.publish(t)
client = CorbaClient()
bag = rosbag.Bag("/usr/localDev/rosbag-calib/pyrene-calib.bag")
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
robot = Robot("talos", "talos", rootJointType="freeflyer", client=client)
q = robot.getCurrentConfig()
pub_cMo = rospy.Publisher("/camera_object", TransformROS, queue_size=100)
pub_fMe = rospy.Publisher("/world_effector", TransformROS, queue_size=100)
rospy.init_node("pose_sender", anonymous=True)
i = 0
for (_, joint_states, _), (_, checkerboard_pose,
_) in zip(bag.read_messages(topics=["joints"]),
bag.read_messages(topics=["chessboard"])):
root_joint_rank = robot.rankInConfiguration["talos/root_joint"]
q[root_joint_rank:root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
joints_name_value_tuple = zip(joint_states.name, joint_states.position)
def __init__ (self, compositeName, robotName, load = True,
rootJointType = "planar", **kwargs):
Parent.__init__ (self, compositeName, robotName, rootJointType, load, **kwargs)
from hpp.corbaserver import Client
from hpp.corbaserver.manipulation import Client as ManipClient
cl = Client()
mcl = ManipClient()
mcl.robot.create ("test")
cl.robot.appendJoint ("", "A/root_joint", "planar", [0,0,0,0,0,0,1])
cl.robot.createSphere ("A/root_body", 0.001)
cl.robot.addObjectToJoint ("A/root_joint", "A/root_body", [0,0,1,0,0,0,1])
mcl.robot.finishedRobot ("A")
cl.robot.appendJoint ("", "B/root_joint", "planar", [0,0,0,0,0,0,1])
cl.robot.createBox ("B/root_body", 0.001, 0.002, 0.004)
cl.robot.addObjectToJoint ("B/root_joint", "B/root_body", [0,0,1,0,0,0,1])
mcl.robot.finishedRobot ("B")
from hpp.corbaserver.manipulation.robot import Robot
robot = Robot()
def __init__ (self, compositeName, robotName, load = True,
rootJointType = "planar"):
Parent.__init__ (self, compositeName, robotName, rootJointType, load)
self.tf_root = "base_footprint"
srdfSuffix = ""
pose = "pose"
handles = []
contacts = [
"top",
]
newProblem()
Robot.packageName = 'talos_data'
Robot.urdfName = 'talos'
Robot.urdfSuffix = '_full_v2'
Robot.srdfSuffix = ''
robot = Robot('dev', 'talos', rootJointType="freeflyer")
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("Graph-RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
def __init__ (self, compositeName, robotName, load = True,
rootJointType = "anchor"):
Parent.__init__ (self, compositeName, robotName, rootJointType, load)
self.tf_root = "base_footprint"
def __init__(self, agentName):
print 'initialising a HyQ agent'
Robot.__init__(self, 'meta', agentName, "planar")
