def __init__(self):
from threading import Lock
# Client to HPP
self.client = HppClient(context="simulation")
self.mutex = Lock()
# Initialize configuration of robot and objects from HPP
hpp = self.client.hpp()
self.q = hpp.problem.getInitialConfig()
self.configSize = len(self.q)
# Compute ranks of joints in HPP configuration
self.computeRanksInConfiguration()
self.initializeObjects(self.q)
self.initializeSoT2HPPconversion()
self.transitionName = ""
self.q_rhs = None
self.objectPublisher = dict()
# Create an object publisher by object
prefix = rospy.get_param('tf_prefix', 'sim_')
for o in self.objects:
self.objectPublisher [o] = PublishObjectPose \
('world', prefix+o+'/base_link')
self.objectPublisher[o].broadcast(self.objectPose[o])
pose = self.objectPose[o]
r = self.rankInConfiguration[o + '/root_joint']
self.q[r:r + 7] = pose
# Create subscribers
self.subscribers = ros_tools.createSubscribers(self, "/agimus",
self.subscriberDict)
# Create mapping from transition names to graph component id in HPP
self.graphDict = createGraphDict(self.client)
class InitializePath(smach.State):
hppTargetPubDict = {"read_subpath": [ReadSubPath, 1]}
serviceProxiesDict = {
"agimus": {
"sot": {
"clear_queues": [std_srvs.srv.Trigger]
}
},
"hpp": {
"target": {
"publish_first": [std_srvs.srv.Trigger]
}
},
}
def __init__(self):
super(InitializePath, self).__init__(
outcomes=_outcomes,
input_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
],
output_keys=[
"transitionId", "endStateId", "currentSection", "duration"
],
)
self.targetPub = ros_tools.createPublishers("/hpp/target",
self.hppTargetPubDict)
self.serviceProxies = ros_tools.createServiceProxies(
"", InitializePath.serviceProxiesDict)
self.hppclient = HppClient(context="corbaserver")
def execute(self, userdata):
userdata.currentSection += 1
if userdata.currentSection + 1 >= len(userdata.times):
return _outcomes[0]
start = userdata.times[userdata.currentSection]
length = userdata.times[userdata.currentSection + 1] - start
transitionId = userdata.transitionIds[userdata.currentSection]
userdata.transitionId = transitionId
userdata.endStateId = userdata.endStateIds[userdata.currentSection]
userdata.duration = length
wait_if_step_by_step("Preparing to read subpath.", 3)
manip = self.hppclient._manip()
manip.graph.selectGraph(transitionId[1])
self.targetPub["read_subpath"].publish(
ReadSubPath(userdata.pathId, start, length))
rospy.loginfo("Start reading subpath.")
return _outcomes[2]
def __init__(self):
super(InitializePath, self).__init__(
outcomes=_outcomes,
input_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
],
output_keys=[
"transitionId", "endStateId", "currentSection", "duration"
],
)
self.targetPub = ros_tools.createPublishers("/hpp/target",
self.hppTargetPubDict)
self.serviceProxies = ros_tools.createServiceProxies(
"", InitializePath.serviceProxiesDict)
self.hppclient = HppClient(context="corbaserver")
def __init__(self):
super(WaitForInput, self).__init__(
outcomes=["succeeded", "aborted"],
input_keys=[],
output_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
"queue_initialized",
],
)
rospy.logwarn("Create service WaitForInput")
self.services = ros_tools.createServiceProxies("",
self.serviceProxiesDict)
rospy.logwarn("f**k")
self.status_srv = rospy.Service("status", std_srvs.srv.Trigger,
self.getStatus)
rospy.logwarn("you")
self.hppclient = HppClient(context="corbaserver")
self.ready = False
self.status = "not started"
class WaitForInput(smach.State):
# Accessed services
serviceProxiesDict = {
"agimus": {
"sot": {
"request_hpp_topics": [std_srvs.srv.Trigger],
"plug_sot": [PlugSot],
"set_base_pose": [SetPose],
}
},
"hpp": {
"target": {
"reset_topics": [std_srvs.srv.Empty]
}
},
}
def __init__(self):
super(WaitForInput, self).__init__(
outcomes=["succeeded", "aborted"],
input_keys=[],
output_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
"queue_initialized",
],
)
rospy.logwarn("Create service WaitForInput")
self.services = ros_tools.createServiceProxies("",
self.serviceProxiesDict)
rospy.logwarn("f**k")
self.status_srv = rospy.Service("status", std_srvs.srv.Trigger,
self.getStatus)
rospy.logwarn("you")
self.hppclient = HppClient(context="corbaserver")
self.ready = False
self.status = "not started"
def getStatus(self, empty):
return self.ready, self.status
def execute(self, userdata):
self.status = "waiting"
self.ready = True
res = rospy.wait_for_message("start_path", UInt32)
self.ready = False
pid = res.data
self.status = "playing path " + str(pid)
rospy.loginfo("Requested to start path " + str(pid))
userdata.pathId = pid
userdata.queue_initialized = False
try:
hpp = self.hppclient._hpp()
manip = self.hppclient._manip()
qs, ts = hpp.problem.getWaypoints(pid)
# Add a first section to force going to init pose.
# ts: list of transition times. The first one is repeated to make the robot
# move to the initial configuration before any other motion.
ts = ts[0:1] + ts
# tids: list of pair (transitionName, graphName)
tids = [
manip.problem.edgeAtParam(pid, (t0 + t1) / 2)
for t0, t1 in zip(ts[:-1], ts[1:])
]
for i, tid in enumerate(tids):
manip.graph.selectGraph(tid[1])
tids[i] = (manip.graph.getName(tid[0]), tid[1])
# print len(qs), len(ts), len(tids)
# Remove fake transitions (i.e. when id is the same for two consecutive transitions)
tts = ts[0:3]
ttids = tids[0:2]
tqs = qs[0:2]
for t, id, q in zip(ts[2:], tids[1:], qs[1:]):
if ttids[-1] == id:
tts[-1] = t
tqs[-1] = q
else:
ttids.append(id)
tts.append(t)
tqs.append(q)
userdata.times = tuple(tts)
userdata.transitionIds = tuple(ttids)
endStateIds = []
for q, tid in zip(tqs, ttids):
manip.graph.selectGraph(tid[1])
nid = manip.graph.getNode(q)
endStateIds.append((manip.graph.getName(nid), tid[1]))
userdata.endStateIds = tuple(endStateIds)
userdata.currentSection = -1
# TODO reset_topics should not be necessary
self.services["hpp"]["target"]["reset_topics"]()
self.services["agimus"]["sot"]["request_hpp_topics"]()
# Set base pose
# TODO shouldn't this be done by the estimation node ?
# moreover, this assumes that HPP has a free-floating base.
from agimus_hpp.tools import hppPoseToSotTransRPY
self.services["agimus"]["sot"]["set_base_pose"](
*hppPoseToSotTransRPY(tqs[0][:7]))
rospy.sleep(0.001)
# TODO check that qs[0] and the current robot configuration are
# close
except Exception as e:
rospy.logerr("Failed " + str(e))
return "aborted"
return "succeeded"
def makeStateMachine():
from .status import Status
# Set default values of parameters
if not rospy.has_param("step_by_step"):
rospy.set_param("step_by_step", 0)
sm = smach.StateMachine(outcomes=["aborted",])
status = Status()
with sm:
from agimus_hpp.client import HppClient
hppclient = HppClient(context="corbaserver", connect=False)
smach.StateMachine.add(
"WaitForInput",
WaitForInput(status, hppclient),
transitions={
"start_path": "Init",
"failed_to_start": "WaitForInput",
"interrupted": "aborted"},
remapping={
"pathId": "pathId",
"times": "times",
"transitionIds": "transitionIds",
"endStateIds": "endStateIds",
"currentSection": "currentSection",
"queue_initialized": "queue_initialized",
},
)
smach.StateMachine.add(
"Init",
InitializePath(status, hppclient),
transitions={
"finished": "WaitForInput",
"move_base": "MoveBase",
"next": "Play",
},
remapping={
"pathId": "pathId",
"times": "times",
"transitionId": "transitionId",
"currentSection": "currentSection",
},
)
smach.StateMachine.add(
"MoveBase",
MoveBase(status),
transitions={
"succeeded": "Init",
"preempted": "Error",
},
remapping={
"currentSection": "currentSection",
"times": "times",
"pathId": "pathId",
},
)
smach.StateMachine.add(
"Play",
PlayPath(status),
transitions={
"succeeded": "Init",
"aborted": "WaitForInput",
"preempted": "Error",
},
remapping={
"transitionId": "transitionId",
"duration": "duration",
"currentSection": "currentSection",
"queue_initialized": "queue_initialized",
},
)
smach.StateMachine.add(
"Error",
ErrorState(status),
transitions={
"finished": "WaitForInput",
},
)
sm.set_initial_state(["WaitForInput"])
sis = smach_ros.IntrospectionServer("agimus", sm, "/AGIMUS")
return sm, sis
class Simulation(object):
subscriberDict = {
"sot": {
"transition_name": [String, "computeObjectPositions"],
"state": [Vector, "getRobotState"],
},
}
def computeRanksInConfiguration(self):
# Compute ranks of joints in HPP configuration vector
hpp = self.client.hpp()
self.rankInConfiguration = dict()
joints = hpp.robot.getAllJointNames()
rank = 0
for j in joints:
size = hpp.robot.getJointConfigSize(j)
if size != 0:
self.rankInConfiguration[j] = rank
rank += size
def initializeObjects(self, qinit):
hpp = self.client.hpp()
joints = hpp.robot.getJointNames()
self.robotName = joints[0].split('/', 1)[0]
self.objects = {j.split('/', 1)[0] for j in joints}
self.objects.remove(self.robotName)
self.objectPose = {}
# Compute objects position
for jn in joints:
obj = jn.split('/', 1)[0]
if obj in self.objects:
iq = self.rankInConfiguration[jn]
nq = hpp.robot.getJointConfigSize(jn)
if obj not in self.objectPose:
self.objectPose[obj] = qinit[iq:iq + nq]
else:
self.objectPose[obj].extend(qinit[iq:iq + nq])
rospy.loginfo("objects pose: " + str(self.objectPose))
def initializeSoT2HPPconversion(self):
from agimus_sot_msgs.srv import GetJointNames
get_joint_names = ros_tools.wait_for_service(
"/agimus/sot/get_joint_names", GetJointNames)
self.sot_joint_names = get_joint_names().names
assert self.sot_joint_names[0] == "root_joint"
self.sot2hpp_slices = []
hpp = self.client.hpp()
# Special case for the root joint
rjt = hpp.robot.getJointType(self.robotName + "/root_joint")
if rjt == "JointModelFreeFlyer":
self.sot2hpp_rootJointConversion = sotTransRPYToHppPose
elif rjt == "JointModelPlanar":
from math import cos, sin
self.sot2hpp_rootJointConversion = lambda x: list(
x[:2]) + [cos(x[5]), sin(x[5])]
else:
rospy.logerr(
"Cannot convert the root joint configuration into HPP root joint type {}."
.format(rjt))
srk = 6
for sj in self.sot_joint_names[1:]:
hj = self.robotName + "/" + sj
if hj not in self.rankInConfiguration:
rospy.logwarn(
"SoT joint {} does not correspond to any HPP joint".format(
sj))
else:
hrk = self.rankInConfiguration[hj]
hsz = hpp.robot.getJointConfigSize(hj)
hslice = slice(hrk, hrk + hsz)
sslice = slice(srk, srk + hsz)
self.sot2hpp_slices.append((sslice, hslice))
srk += hsz
def __init__(self):
from threading import Lock
# Client to HPP
self.client = HppClient(context="simulation")
self.mutex = Lock()
# Initialize configuration of robot and objects from HPP
hpp = self.client.hpp()
self.q = hpp.problem.getInitialConfig()
self.configSize = len(self.q)
# Compute ranks of joints in HPP configuration
self.computeRanksInConfiguration()
self.initializeObjects(self.q)
self.initializeSoT2HPPconversion()
self.transitionName = ""
self.q_rhs = None
self.objectPublisher = dict()
# Create an object publisher by object
prefix = rospy.get_param('tf_prefix', 'sim_')
for o in self.objects:
self.objectPublisher [o] = PublishObjectPose \
('world', prefix+o+'/base_link')
self.objectPublisher[o].broadcast(self.objectPose[o])
pose = self.objectPose[o]
r = self.rankInConfiguration[o + '/root_joint']
self.q[r:r + 7] = pose
# Create subscribers
self.subscribers = ros_tools.createSubscribers(self, "/agimus",
self.subscriberDict)
# Create mapping from transition names to graph component id in HPP
self.graphDict = createGraphDict(self.client)
def getRobotState(self, msg):
self.mutex.acquire()
try:
# Convert RPY to quaternion
rjq = self.sot2hpp_rootJointConversion(msg.data[:6])
self.q[:len(rjq)] = rjq
for sslice, hslice in self.sot2hpp_slices:
self.q[hslice] = msg.data[sslice]
# update poses of objects
for o in self.objects:
pose = self.objectPose[o]
self.objectPublisher[o].broadcast(pose)
except Exception:
rospy.logerr(traceback.format_exc())
finally:
self.mutex.release()
def computeObjectPositions(self, msg):
if msg.data == "": return
self.mutex.acquire()
try:
transitionChanged = msg.data != self.transitionName
self.transitionName = msg.data
# if transition changed, record configuration for transition constraint
# right hand side
if transitionChanged:
self.q_rhs = self.q
self.transitionId = self.graphDict[self.transitionName]
print("new transition: " + self.transitionName)
print("q_rhs = " + str(self.q_rhs))
elif self.q_rhs:
# if transition has not changed and right hand side has been stored,
# apply transition constraints
res, self.q, err = \
self.client.manip ().graph.applyEdgeLeafConstraints \
(self.transitionId, self.q_rhs, self.q)
self.q = list(self.q)
for o in self.objects:
r = self.rankInConfiguration[o + '/root_joint']
self.objectPose[o] = self.q[r:r + 7]
except Exception as e:
rospy.logerr(e)
finally:
self.mutex.release()