hrpsys_gazebo_atlas: atlas_client.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import imp  ## for rosbuild
00004 try:
00005     imp.find_module("hrpsys")
00006 except:
00007     import roslib; roslib.load_manifest("hrpsys")
00008 
00009 import hrpsys
00010 from hrpsys.hrpsys_config import *
00011 import OpenHRP
00012 
00013 class ATLASHrpsysConfigurator(HrpsysConfigurator):
00014 #    tjc = None
00015 
00016     def connectComps(self):
00017         HrpsysConfigurator.connectComps(self)
00018         #
00019         #disconnectPorts(self.st.port("q"),  self.co.port("qRef"))
00020         #disconnectPorts(self.el.port("q"),  self.rh.port("qRef"))
00021         #connectPorts(self.st.port("q"),  self.rh.port("qRef"))
00022         #connectPorts(self.st.port("q"), self.rh.port("qRef"))
00023         #self.tjc = self.createComp("TendonJointController", "tjc")
00024         #connectPorts(self.st.port("q"),  self.tjc.port("qRef"))
00025         #connectPorts(self.tjc.port("q"),  self.rh.port("qRef"))
00026 
00027     def getRTCList(self):
00028 #        return [self.rh, self.seq, self.sh, self.tf, self.kf, self.afs, self.ic, self.abc, self.log]
00029 #        return [self.rh, self.seq, self.sh, self.fk, self.tf, self.kf, self.vs, self.afs, self.ic, self.abc, self.st, self.log]
00030         #return [self.rh, self.seq, self.sh, self.fk, self.tf, self.kf, self.vs, self.afs, self.ic, self.abc, self.st, self.tjc, self.log]
00031         #return [self.rh, self.seq, self.sh, self.fk, self.tf, self.kf, self.vs, self.afs, self.ic, self.abc, self.st, self.tjc, self.log]
00032         #return [self.rh, self.seq, self.sh, self.fk, self.tf, self.kf, self.vs, self.afs, self.ic, self.abc, self.st, self.log]
00033         return [
00034             ['seq', "SequencePlayer"],
00035             ['sh', "StateHolder"],
00036             ['fk', "ForwardKinematics"],
00037             ['tf', "TorqueFilter"],
00038             ['kf', "KalmanFilter"],
00039             ['vs', "VirtualForceSensor"],
00040             ['rmfo', "RemoveForceSensorLinkOffset"],
00041             ['ic', "ImpedanceController"],
00042             ['abc', "AutoBalancer"],
00043             ['st', "Stabilizer"],
00044             #['co', "CollisionDetector"],
00045             #['el', "SoftErrorLimiter"],
00046             ['log', "DataLogger"]
00047             ]
00048         
00049 
00050     def init(self, robotname="Robot", url=""):
00051         HrpsysConfigurator.init(self, robotname, url)
00052         time.sleep(6)
00053         rh_svc = narrow(self.rh.service("service0"), "RobotHardwareService")
00054         rh_svc.setServoErrorLimit("all", 0); # disable RobotHardware Joint Error check
00055 
00056     def setSelfGroups(self):
00057         '''
00058         Set elements of body groups and joing groups that are statically
00059         defined as member variables within this class.
00060         '''
00061         for item in self.Groups:
00062             self.seq_svc.addJointGroup(item[0], item[1])
00063 
00064     def resetJointGroup(self):
00065         # remove jointGroups
00066         self.seq_svc.removeJointGroup("torso")
00067         self.seq_svc.removeJointGroup("head")
00068         self.seq_svc.removeJointGroup("larm")
00069         self.seq_svc.removeJointGroup("rarm")
00070         self.seq_svc.removeJointGroup("lleg")
00071         self.seq_svc.removeJointGroup("rleg")
00072         # setup jointGroups
00073         self.setSelfGroups() # restart groups
00074 
00075     def setupLogger(self):
00076         HrpsysConfigurator.setupLogger(self)
00077         #default = 4000 too shoort!
00078         self.log_svc.maxLength(400000)
00079 
00080 if __name__ == '__main__':
00081     ATLASHrpsysConfigurator().init(sys.argv[1], sys.argv[2])