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()