def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
sh = initRTC("StateHolder", "StateHolder0")
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
kf = initRTC("KalmanFilter", "kf")
kpg = initRTC("WalkGenerator", "kpg")
kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(kpg.service("service0"))
st = initRTC("Stabilizer", "st")
st.setConfiguration(bodyinfo.hstConfig)
log = initRTC("DataLogger", "log")
return [rh, seq, kf, kpg, sh, st, log]
def demoDualarmPush (auto_detecion = True):
print >> sys.stderr, "3. Dual-arm push demo."
print >> sys.stderr, " Move to"
hcf.abc_svc.goPos(-0.45,0,0);
hcf.abc_svc.waitFootSteps();
hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
hcf.abc_svc.waitFootSteps();
print >> sys.stderr, " Reaching"
#hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
hcf.waitInterpolation()
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Increase operational force"
if auto_detecion:
objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
else:
hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
hcf.waitInterpolation()
print >> sys.stderr, " Push forward"
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode = True
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.5,0,0)
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setWrenches([0]*24, 2.0)
hcf.waitInterpolation()
def init(hostname=socket.gethostname()):
global ms, simulation_mode, kinematics_mode
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh is not None:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
servo = rh
ep_svc = narrow(rh.ec, "ExecutionProfileService")
simulation_mode = 0
else:
rh = rtm.findRTC("JVRC1Controller(Robot)0")
servo = rtm.findRTC("PDcontroller0")
simulation_mode = 1
if rh.port("baseTformIn"):
kinematics_mode = 1
else:
kinematics_mode = 0
print("creating components")
createComps()
print("connecting components")
connectComps()
print("activating components")
activateComps()
print("initialized successfully")
def demoDualarmPush (auto_detecion = True):
print >> sys.stderr, "3. Dual-arm push demo."
print >> sys.stderr, " Move to"
hcf.abc_svc.goPos(-0.45,0,0);
hcf.abc_svc.waitFootSteps();
hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
hcf.abc_svc.waitFootSteps();
print >> sys.stderr, " Reaching"
#hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
hcf.waitInterpolation()
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Increase operational force"
if auto_detecion:
objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
else:
hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
hcf.waitInterpolation()
print >> sys.stderr, " Push forward"
hcf.abc_svc.goPos(0.5,0,0)
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setWrenches([0]*24, 2.0)
hcf.waitInterpolation()
def createComps():
global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
global ep_svc, mc, mc_svc
rh = rtm.findRTC("RobotHardware0")
if rh is not None:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
servo = rh
ep_svc = narrow(rh.ec, "ExecutionProfileService")
else:
rh = rtm.findRTC("JVRC1Controller(Robot)0")
servo = rtm.findRTC("PDcontroller0")
ms = rtm.findRTCmanager()
ms.load("KalmanFilter")
kf = ms.create("KalmanFilter")
ms.load("StateHolder")
sh = ms.create("StateHolder")
sh_svc = narrow(sh.service("service0"), "StateHolderService")
tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
ms.load("DataLogger")
log = ms.create("DataLogger")
log_svc = narrow(log.service("service0"), "DataLoggerService")
ms.load("MCControl")
mc = ms.create("MCControl")
def createComps():
global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
global ep_svc, mc, mc_ctrl
rh = rtm.findRTC("pandaController(Robot)0")
servo = rtm.findRTC("PDcontroller0")
ms = rtm.findRTCmanager()
ms.load("KalmanFilter")
kf = ms.create("KalmanFilter")
ms.load("StateHolder")
sh = ms.create("StateHolder")
sh_svc = narrow(sh.service("service0"), "StateHolderService")
tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
ms.load("DataLogger")
log = ms.create("DataLogger")
log_svc = narrow(log.service("service0"), "DataLoggerService")
if use_udp:
ms.load("MCUDPSensors")
mc = ms.create("MCUDPSensors")
ms.load("MCUDPControl")
mc_ctrl = ms.create("MCUDPControl")
else:
ms.load("MCControl")
mc = ms.create("MCControl")
def init(host='localhost'):
global vs, vs_svc, cvp, cvp_svc
global vs_head, vs_head_svc, cvp_head, cvp_head_svc
sample.init(host)
# for hand
vs = rtm.findRTC("VideoStream0_hand")
if vs != None:
vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0'))
vs.start()
cvp = rtm.findRTC("CvProcessor0_hand")
cvp_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp.service('service0'))
cvp.start()
rtm.connectPorts(vs.port("MultiCameraImages"), cvp.port("MultiCameraImage"))
vs_svc.take_one_frame()
# for head
vs_head = rtm.findRTC("VideoStream0_head")
if vs_head != None:
vs_head_svc = Img.CameraCaptureServiceHelper.narrow(vs_head.service('service0'))
vs_head.start()
cvp_head = rtm.findRTC("CvProcessor0_head")
cvp_head_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp_head.service('service0'))
cvp_head.start()
rtm.connectPorts(vs_head.port("MultiCameraImages"), cvp_head.port("MultiCameraImage"))
vs_head_svc.take_one_frame()
time.sleep(1)
def init():
global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
ep_svc = narrow(rh.ec, "ExecutionProfileService")
qRefPort = rh.port("qRef")
else:
rh = rtm.findRTC("PA10Controller(Robot)0")
qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
simulation_mode = 1
simulation_mode = 0
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def init():
global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
ep_svc = narrow(rh.ec, "ExecutionProfileService")
qRefPort = rh.port("qRef")
else:
rh = rtm.findRTC("PA10Controller(Robot)0")
qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
simulation_mode = 1
simulation_mode = 0
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def init():
global vs_svc, vs_port, md_port
vs = rtm.findRTC('VideoStream0')
vs.start()
vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0'))
vs_port = vs.port('MultiCameraImages')
# if you setup MultiDisp RTC (OpenVGR)
md = rtm.findRTC('MultiDisp0')
if md != None:
md_port = md.port('images')
rtm.connectPorts(vs_port, md_port)
md.start()
def createComps(hostname=socket.gethostname()):
global ms, user, user_svc, log, rh, servo, joystick, kf, st, st_svc
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("HRP2A")
if ms==None:
print "no ms"
else:
user = initRTC("sony", "wpg")
joystick= initRTC("GamepadRTC","joystick")
kf= initRTC("KalmanFilter","kf")
st= initRTC("Stabilizer","st")
#log = initRTC("DataLogger", "log")
servo= rtm.findRTC("JojoPDservo0")
#user = rtm.findRTC("sony0")
#joystick= rtm.findRTC("GamepadRTC0")
#kf=rtm.findRTC("KalmanFilter0")
#st=rtm.findRTC("Stabilizer0")
if servo==None:
print "no servo component"
return
if rh==None:
print "no robot component"
return
if user==None:
print "no user component"
return
user_svc = OpenHRP.sonyServiceHelper.narrow(user.service("service0"))
if user_svc==None:
print "no svc"
st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
if st_svc==None:
print "no st svc"
if joystick==None:
print "no joystick component"
rtcs=[rh, joystick, kf, user, st]
#user.start()
#joystick.start()
#return [rh, user]
return rtcs
def rtc_init():
global ms, co, co_svc, root_link_name, root_link_offset
initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None:
time.sleep(1)
ms = rtm.findRTCmanager(rtm.nshost)
rospy.loginfo("[collision_state.py] wait for RTCmanager : ", ms)
co = rtm.findRTC(rospy.get_param('~comp_name', 'co'))
if co == None:
rospy.logerr("Could not found CollisionDetector, exiting...")
exit(1)
co_svc = narrow(co.service("service0"), "CollisionDetectorService")
if modelfile:
#import CosNaming
obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
mdlldr = obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
rospy.loginfo(" bodyinfo URL = file://" + modelfile)
body_info = mdlldr.getBodyInfo("file://" + modelfile)
root_link_name = body_info._get_links()[0].name
root_link_offset = inverse_matrix(
concatenate_matrices(
translation_matrix(body_info._get_links()[0].translation),
rotation_matrix(body_info._get_links()[0].rotation[3],
body_info._get_links()[0].rotation[0:3])))
else:
root_link_name = "WAIST"
root_link_offset = identity_matrix()
rospy.loginfo("ssetup collision_state with " + root_link_name + " " +
root_link_offset)
def createComps(hostname=socket.gethostname()):
global ms, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
rh_svc = None
if rh != None:
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
else:
rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
sh = initRTC("StateHolder", "StateHolder0")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
return [rh, seq, sh]
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode, sot
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(
rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(
adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC(bodyinfo.modelName + "Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(
seq.service("service0"))
sh = initRTC("StateHolder", "StateHolder0")
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
if useStabilizerComp:
kf = initRTC("KalmanFilter", "kf")
kpg = initRTC("WalkGenerator", "kpg")
kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(
kpg.service("service0"))
if useStabilizerComp:
st = initRTC("Stabilizer", "st")
st.setConfiguration(bodyinfo.hstConfig)
log = initRTC("DataLogger", "log")
print "Creating SoT"
sot = initRTC("RtcStackOfTasks", "sot")
print "Set configuration of SoT"
sot.setConfiguration(sotinfo.sotConfig)
if useStabilizerComp:
return [rh, seq, kf, kpg, sh, st, log, sot]
else:
return [rh, seq, kpg, sh, log, sot]
def findComp(self, compName, instanceName, max_timeout_count = 10):
timeout_count = 0;
comp = rtm.findRTC(instanceName)
while comp == None and timeout_count < max_timeout_count:
comp = rtm.findRTC(instanceName)
if comp != None:
break
print self.configurator_name, " find Comp wait for", instanceName
time.sleep(1)
timeout_count += 1
print self.configurator_name, " find Comp : ", instanceName, " = ", comp
if comp == None:
print self.configurator_name, " Cannot find component: " + instanceName + " (" + compName +")"
return [None, None]
if comp.service("service0"):
comp_svc = narrow(comp.service("service0"), compName+"Service")
print self.configurator_name, " find CompSvc : ", instanceName + "_svc = ", comp_svc
return [comp, comp_svc]
else:
return [comp, None]
def findComp(self, compName, instanceName, max_timeout_count = 10):
timeout_count = 0;
comp = rtm.findRTC(instanceName)
while comp == None and timeout_count < max_timeout_count:
comp = rtm.findRTC(instanceName)
if comp != None:
break
print self.configurator_name, " find Comp wait for", instanceName
time.sleep(1)
timeout_count += 1
print self.configurator_name, " find Comp : ", instanceName, " = ", comp
if comp == None:
print self.configurator_name, " Cannot find component: " + instanceName + " (" + compName +")"
return [None, None]
if comp.service("service0"):
comp_svc = narrow(comp.service("service0"), compName+"Service")
print self.configurator_name, " find CompSvc : ", instanceName + "_svc = ", comp_svc
return [comp, comp_svc]
else:
return [comp, None]
def createCompsConsole(nshC, rncC):
global msC, rhC, servoC, rtcListC
msC = rtm.findRTCmanager(nshC, rncC)
rhC = rtm.findRTC("JVRC-State", rncC)
servoC = rtm.findRTC("creekRobotState0", rncC)
if rhC == None:
rtcListC = rtcListR
servoC = initRTC("creekRobotState", "creekRobotState0", msC)
else:
rtcListC = [rhC]
#rtcListC = rtcListR
if servoC == None:
return
global camera, camera_svc
camera = initRTC("creekCameraViewer", "camera", msC)
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcListC.append(camera)
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
self.assertTrue(ms and rh)
except Exception as e:
print "{0}, RTCmanager={1}, RTC(RobotHardware0)={2}".format(str(e),ms,rh)
self.fail()
pass
def init(host='localhost', refSuffix=""):
global vs, vs_svc, cvp, cvp_svc
if robotHost != None:
print 'robot host = '+robotHost
java.lang.System.setProperty('NS_OPT',
'-ORBInitRef NameService=corbaloc:iiop:'+robotHost+':2809/NameService')
rtm.initCORBA()
if refSuffix == None:
refSuffix = ''
elif refSuffix != '':
refSuffix = '_'+refSuffix
print 'suffix = '+refSuffix
vs = rtm.findRTC("VideoStream0"+refSuffix)
vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0'))
vs.start()
cvp = rtm.findRTC("CvProcessor0"+refSuffix)
cvp_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp.service('service0'))
cvp.start()
rtm.connectPorts(vs.port("MultiCameraImages"), cvp.port("MultiCameraImage"))
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, armR, armR_svc,\
armL, armL_svc, grsp, grsp_svc, sa, tk_svc, log, log_svc, gf, ca, use_ros
ms = rtm.findRTCmanager(hostname)
adm = rtm.findRTC("SystemAdmin0")
adm_svc = None
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
rh = rtm.findRTC("RobotHardware0")
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
if dir(seq_svc).count("loadTrajectory") == 0:
seq_svc = SequencePlayerRosService(seq_svc, rh_svc)
armR = initRTC("ArmControl", "armR")
armR_svc = OpenHRP.ArmControlServiceHelper.narrow(armR.service("service0"))
armL = initRTC("ArmControl", "armL")
armL_svc = OpenHRP.ArmControlServiceHelper.narrow(armL.service("service0"))
grsp = initRTC("Grasper", "grsp")
grsp_svc = OpenHRP.GrasperServiceHelper.narrow(grsp.service("service0"))
sa = initRTC("StateArbitrator", "sa")
tk_svc = OpenHRP.TimeKeeperServiceHelper.narrow(sa.service("service1"))
log = initRTC("DataLogger", "log")
log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0"))
gf = initRTC("GazeFixer", "gf")
ca = initRTC("CollisionAvoider", "ca")
return [rh, seq, armR, armL, sa, grsp, gf, ca, log]
def waitForRobotHardware(self, robotname="Robot"):
self.rh = None
timeout_count = 0;
# wait for simulator or RobotHardware setup which sometime takes a long time
while self.rh == None and timeout_count < 10: # <- time out limit
time.sleep(1);
self.rh = rtm.findRTC("RobotHardware0")
if not self.rh:
self.rh = rtm.findRTC(robotname)
print self.configurator_name, "wait for", robotname, " : ",self.rh, "(timeout ", timeout_count, " < 10)"
if self.rh and self.rh.isActive() == None: # just in case rh is not ready...
self.rh = None
timeout_count += 1
if not self.rh:
print self.configurator_name, "Could not find ", robotname
if self.ms:
print self.configurator_name, "Candidates are .... ", [x.name() for x in self.ms.get_components()]
print self.configurator_name, "Exitting.... ", robotname
exit(1)
print self.configurator_name, "findComps -> RobotHardware : ",self.rh, "isActive? = ", self.rh.isActive()
def waitForRobotHardware(self, robotname="Robot"):
self.rh = None
timeout_count = 0;
# wait for simulator or RobotHardware setup which sometime takes a long time
while self.rh == None and timeout_count < 10: # <- time out limit
time.sleep(1);
self.rh = rtm.findRTC("RobotHardware0")
if not self.rh:
self.rh = rtm.findRTC(robotname)
print self.configurator_name, "wait for", robotname, " : ",self.rh, "(timeout ", timeout_count, " < 10)"
if self.rh and self.rh.isActive() == None: # just in case rh is not ready...
self.rh = None
timeout_count += 1
if not self.rh:
print self.configurator_name, "Could not find ", robotname
if self.ms:
print self.configurator_name, "Candidates are .... ", [x.name() for x in self.ms.get_components()]
print self.configurator_name, "Exitting.... ", robotname
exit(1)
print self.configurator_name, "findComps -> RobotHardware : ",self.rh, "isActive? = ", self.rh.isActive()
def createCompsRobot(nshR, rncR):
global ms, rh, servo, rtcListR
ms = rtm.findRTCmanager(nshR, rncR)
rh = rtm.findRTC(bodyinfo.modelName, rncR)
servo = rtm.findRTC("creekPdServo0", rncR)
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcListR = [rh]
global kf, seq, seq_svc, sh, wpg, wpg_svc, st, st_svc, arm, arm_svc
kf = initRTC("creekStateEstimator", "kf", ms)
if kf != None:
rtcListR.append(kf)
seq = initRTC("creekSequencePlayer", "seq", ms)
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcListR.append(seq)
wpg = initRTC("sony", "wpg", ms)
if wpg != None:
wpg_svc = OpenHRP.sonyServiceHelper.narrow(wpg.service("service0"))
rtcListR.append(wpg)
arm = initRTC("creekArmControlCartesian", "arm", ms)
if arm != None:
arm_svc = OpenHRP.creekArmControlCartesianServiceHelper.narrow(arm.service("service0"))
rtcListR.append(arm)
sh = initRTC("creekReferenceHolder", "holder", ms)
if sh != None:
rtcListR.append(sh)
st= initRTC("Stabilizer","st", ms)
if st != None:
st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
rtcListR.append(st)
def createComps(hostname=socket.gethostname()):
global ms, rh, servo, rtcList
global rh_s, servo_s
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC(bodyinfo.modelName)
servo = rtm.findRTC("creekPdServo0")
rh_s = rtm.findRTC("JVRC-State")
servo_s = rtm.findRTC("creekRobotState0")
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcList = [rh]
if servo_s == None:
servo_s = initRTC("creekRobotState", "creekRobotState0")
global kf, seq, seq_svc, sh, camera, camera_svc
kf = initRTC("creekStateEstimator", "kf")
if kf != None:
rtcList.append(kf)
seq = initRTC("creekSequencePlayer", "seq")
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcList.append(seq)
sh = initRTC("creekReferenceHolder", "holder")
if sh != None:
rtcList.append(sh)
camera = initRTC("creekCameraViewer", "camera")
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcList.append(camera)
return rtcList
def init(hostname=socket.gethostname()):
global ms, simulation_mode, kinematics_mode
ms = rtm.findRTCmanager()
rh = rtm.findRTC("pandaController(Robot)0")
servo = rtm.findRTC("PDcontroller0")
simulation_mode = 1
if rh.port("baseTformIn"):
kinematics_mode = 1
else:
kinematics_mode = 0
print("creating components")
createComps()
print("connecting components")
connectComps()
print("activating components")
activateComps()
print("initialized successfully")
def createComps():
global rh, ms, tp, log, log_svc
rh = rtm.findRTC("SimpleFoot0")
ms = rtm.findRTCmanager()
ms.load("TactileInfoProcessor")
tp = ms.create("TactileInfoProcessor")
ms.load("DataLogger")
log = ms.create("DataLogger", "log")
log_svc = narrow(log.service("service0"), "DataLoggerService")
def createCompsConsole(nshC, rncC):
global msC, rhC, servoC, servoC_svc, rtcListC
msC = rtm.findRTCmanager(nshC, rncC)
rhC = rtm.findRTC("JVRC-State", rncC)
servoC = rtm.findRTC("creekRobotState0", rncC)
if rhC == None:
rtcListC = rtcListR
servoC = initRTC("creekRobotState", "creekRobotState0", msC)
rtcListC.append(servoC)
else:
rtcListC = [rhC]
if servoC == None:
return
servoC_svc = OpenHRP.creekRobotStateServiceHelper.narrow(servoC.service("service0"))
global camera, camera_svc, stick, pcl, pcl_svc
camera = None
stick = None
pcl = None
camera = initRTC("creekCameraViewer", "camera", msC)
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcListC.append(camera)
stick = initRTC("GamepadRTC", "stick", msC)
if stick != None:
rtcListC.append(stick)
pcl = initRTC("creekPointCloudViewer", "pcl", msC)
if pcl != None:
pcl_svc = OpenHRP.creekPointCloudViewerServiceHelper.narrow(pcl.service("service0"))
rtcListC.append(pcl)
def createCompsRobot(nshR, rncR):
global ms, rh, servo, rtcListR
ms = rtm.findRTCmanager(nshR, rncR)
rh = rtm.findRTC(bodyinfo.modelName, rncR)
servo = rtm.findRTC("creekPdServo0", rncR)
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcListR = [rh]
global kf, seq, seq_svc, sh
kf = initRTC("creekStateEstimator", "kf", ms)
if kf != None:
rtcListR.append(kf)
seq = initRTC("creekSequencePlayer", "seq", ms)
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcListR.append(seq)
sh = initRTC("creekReferenceHolder", "holder", ms)
if sh != None:
rtcListR.append(sh)
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, servo, seq, seq_svc, kf, sh, simulation_mode
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC("JVRC")
servo = rtm.findRTC("creekPdServo0")
seq = initRTC("creekSequencePlayer", "seq")
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
kf = initRTC("creekStateEstimator", "kf")
sh = initRTC("creekReferenceHolder", "holder")
return [rh, seq, kf, sh]
def createComps():
global rh, ms, ted, log, log_svc
rh = rtm.findRTC("SimpleFoot0")
ms = rtm.findRTCmanager()
ms.load("TactileInfoEdgeDetector")
ted = ms.create("TactileInfoEdgeDetector")
ted.setProperty("foot", "left")
ted.setProperty("Wdin", "100000")
ms.load("DataLogger")
log = ms.create("DataLogger", "log")
log_svc = narrow(log.service("service0"), "DataLoggerService")
def rtc_init () :
global ms, co, co_svc, root_link_name, root_link_offset
rtm.initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None :
time.sleep(1);
ms = rtm.findRTCmanager(rtm.nshost)
rospy.loginfo("[collision_state.py] wait for RTCmanager : ",ms)
co = None
count = 0
while co == None and count < 10:
co_name = rospy.get_param('~comp_name', 'co')
co = rtm.findRTC(co_name)
if co and co.isActive():
break
if co and co.isActive() is False:
rospy.logwarn("CollisionDetector(%s) is not activated, waiting..." % co_name)
else:
rospy.logwarn("Could not found CollisionDetector(%s), waiting..." % co_name)
time.sleep(1)
count += 1
co = None
if co == None:
rospy.logerr("Could not found CollisionDetector, exiting...")
exit(0)
co_svc = rtm.narrow(co.service("service0"), "CollisionDetectorService")
if modelfile:
import CosNaming
obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
mdlldr = obj._narrow(OpenHRP.ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
rospy.loginfo(" bodyinfo URL = file://"+modelfile)
body_info = mdlldr.getBodyInfo("file://"+modelfile)
root_link_name = body_info._get_links()[0].segments[0].name
root_link_offset = inverse_matrix(concatenate_matrices(translation_matrix(body_info._get_links()[0].translation),
rotation_matrix(body_info._get_links()[0].rotation[3],
body_info._get_links()[0].rotation[0:3])))
else:
root_link_name = "WAIST"
root_link_offset = identity_matrix()
rospy.loginfo("ssetup collision_state with " + root_link_name + " " + root_link_offset)
def test_rh_service(self):
try:
rh = rh_svc = None
rtm.nshost = 'localhost'
rtm.nsport = 2809
rtm.initCORBA()
rh = rtm.findRTC("RobotHardware0")
rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
print "RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc)
self.assertTrue(rh and rh_svc)
rh.start()
self.assertTrue(rh.isActive())
self.assertTrue(rh_svc.getStatus())
except Exception as e:
print "{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc)
self.fail()
pass
def test_rh_service(self):
try:
rh = rh_svc = None
rtm.nshost = 'localhost'
rtm.nsport = 2809
rtm.initCORBA()
rh = rtm.findRTC("RobotHardware0")
rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
print "RTC(RobotHardware0)={0}, {1}".format(rh, rh_svc)
self.assertTrue(rh and rh_svc)
rh.start()
self.assertTrue(rh.isActive())
self.assertTrue(rh_svc.getStatus())
except Exception as e:
print "{0}, RTC(RobotHardware0)={1}, {2}".format(
str(e), rh, rh_svc)
self.fail()
pass
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
count = 0
while ( not (ms and rh) ) and count < 10:
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if ms and rh :
break
time.sleep(1)
print >>sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r"%(ms,rh)
count += 1
self.assertTrue(ms and rh)
except Exception as e:
self.fail("%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"%(str(e),nshost,nsport,ms,rh))
pass
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
count = 0
while (not (ms and rh)) and count < 10:
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if ms and rh:
break
time.sleep(1)
print >> sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r" % (
ms, rh)
count += 1
self.assertTrue(ms and rh)
except Exception as e:
self.fail(
"%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"
% (str(e), nshost, nsport, ms, rh))
pass
#!/usr/bin/env python
import roslib; roslib.load_manifest("hrpsys")
import os
import rtm
from rtm import *
from OpenHRP import *
import socket
import time
rh=rtm.findRTC("RobotHardware0")
rhr=rtm.findRTC("RobotHardwareServiceROSBridge")
connectPorts(rhr.port("RobotHardwareService"),rh.port("RobotHardwareService"))
#
# test script for VirtualCamera component
#
# start VirtualCameraComp before running this script
#
import rtm
import commands
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://"+openhrp_dir+"/share/OpenHRP-3.1/sample/project/SampleRobot_inHouse.xml"
vc = rtm.findRTC("VirtualCamera0")
print "vc:",vc
vc.setProperty("project", project)
vc.setProperty("camera", "Robot:VISION_SENSOR1")
vc.start()
def findRTC(name):
rtc = rtm.findRTC(name)
if rtc == None:
raise Exception("Failed to find {}.".format(name))
return rtc
from org.omg.CosNaming.NamingContextPackage import *
from com.sun.corba.se.impl.encoding import EncapsOutputStream
from java.lang import System, Class
from RTC import *
from RTM import *
from OpenRTM import *
from _SDOPackage import *
import string, math, socket, time, sys
global rootnc, nshost
java.lang.System.setProperty('NS_OPT', '-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService')
rtm.initCORBA()
global kobuki
kobuki= rtm.findRTC("MobileRobot0")
if kobuki==None:
print "no robot"
prof = kobuki.port("vel").get_port_profile()
prop = prof.properties
for p in prop:
print "kobu",p.name
if p.name == "dataport.data_type":
print p.value.extract_string()
#
# test script for Simulator and Viewer components
#
# start SimulatorComp and ViewerComp before running this script
#
import rtm
import commands
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/SamplePD.xml"
sim = rtm.findRTC("Simulator0")
print "sim:", sim
sim.setProperty("project", project)
vwr = rtm.findRTC("Viewer0")
print "vwr:", vwr
vwr.setProperty("project", project)
rtm.connectPorts(sim.port("state"), vwr.port("state"))
vwr.start()
sim.start()
#
# test script for Simulator component
#
# start hrpsys-viewer and SimulatorComp before running this script
#
import rtm
import commands
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://"+openhrp_dir+"/share/OpenHRP-3.1/sample/project/Sample.xml"
sim = rtm.findRTC("Simulator0")
print "sim:",sim
sim.setProperty("project", project)
sim.setProperty("useOLV", "1")
sim.start()
import rtm
js = rtm.findRTC("Joystick0")
js.start()
#
# test script for VirtualCamera component
#
# start VirtualCameraComp before running this script
#
import rtm
import commands
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/SampleRobot_inHouse.xml"
vc = rtm.findRTC("VirtualCamera0")
print "vc:", vc
vc.setProperty("project", project)
vc.setProperty("camera", "Robot:VISION_SENSOR1")
vc.start()
ns2 = rtm.orb.string_to_object('corbaloc:iiop:%s:%s/NameService' % (nshost2, nsport2))
nc2 = ns2._narrow(CosNaming.NamingContext)
if practicemode:
mgr2 = mgr
else:
mgr2obj = rtm.orb.string_to_object('corbaloc:iiop:%s:2810/manager' % (nshost2))
mgr2 = rtm.RTCmanager(mgr2obj._narrow(RTM.Manager))
mgr2.load('SequencePlayer')
mgr2.load('StateHolder')
mgr2.load('ForwardKinematics')
mgr2.load('Joystick')
mgr2.load('Joystick2PanTiltAngles')
mgr2.load('MidJaxonController')
midjaxon = rtm.findRTC('MIDJAXON', nc2)
rh = rtm.findRTC('PDcontrollerMIDJAXON0', nc2)
seq = mgr2.create('SequencePlayer', 'seq')
seqsvc = rtm.narrow(seq.service('service0'), 'SequencePlayerService', 'hrpsys.OpenHRP')
sh = mgr2.create('StateHolder', 'sh')
shsvc = rtm.narrow(sh.service('service0'), 'StateHolderService', 'hrpsys.OpenHRP')
fk = mgr2.create('ForwardKinematics', 'fk')
rtm.rootnc.rebind([CosNaming.NameComponent('MIDJAXON', 'rtc')], midjaxon.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('PDcontrollerMIDJAXON0', 'rtc')], rh.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('seq', 'rtc')], seq.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('sh', 'rtc')], sh.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('fk', 'rtc')], fk.ref)
js = mgr.create('Joystick', 'js')
js.setProperty('device', '/dev/input/js0')
#!/usr/bin/env python
import roslib
roslib.load_manifest("hrpsys")
import os
import rtm
from rtm import *
from OpenHRP import *
import socket
import time
rh = rtm.findRTC("RobotHardware0")
rhr = rtm.findRTC("RobotHardwareServiceROSBridge")
connectPorts(rhr.port("RobotHardwareService"), rh.port("RobotHardwareService"))
#
# test script for Simulator component
#
# start hrpsys-viewer and SimulatorComp before running this script
#
import rtm
import commands
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/Sample.xml"
sim = rtm.findRTC("Simulator0")
print "sim:", sim
sim.setProperty("project", project)
sim.setProperty("useOLV", "1")
sim.start()
from org.omg.CosNaming import *
from org.omg.CosNaming.NamingContextPackage import *
from com.sun.corba.se.impl.encoding import EncapsOutputStream
from java.lang import System, Class
from RTC import *
from RTM import *
from OpenRTM import *
from _SDOPackage import *
import string, math, socket, time, sys
global rootnc, nshost
java.lang.System.setProperty(
'NS_OPT',
'-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService')
rtm.initCORBA()
global kobuki
kobuki = rtm.findRTC("MobileRobot0")
if kobuki == None:
print "no robot"
prof = kobuki.port("vel").get_port_profile()
prop = prof.properties
for p in prop:
print "kobu", p.name
if p.name == "dataport.data_type":
print p.value.extract_string()
rtm.nsport = nsport
nsloc = "corbaloc:iiop:%s:%d/NameService" % (rtm.nshost, rtm.nsport)
rtm.rootnc = rtm.orb.string_to_object(nsloc)._narrow(
CosNaming.NamingContext)
try:
import cnoid.Corba
rtm.orb = cnoid.Corba.getORB()
except:
rtm.initCORBA()
try:
setNameserver()
camera = rtm.findRTC("IIDCCamera0")
if camera == None:
raise Exception("Failed to find IIDCCamera0")
viewer = rtm.findRTC("ImageViewer0")
if viewer == None:
raise Exception("Failed to find ImageViewer0")
cpanel = rtm.findRTC("ControlPanel0")
if cpanel == None:
raise Exception("Failed to find ControlPanel0")
connectPorts(cpanel.port("Command"), camera.port("Command"))
connectPorts(camera.port("TimedCameraImage"), viewer.port("images"))
camera.start()
viewer.start()
cpanel.start()
rtm.nsport = nsport
nsloc = "corbaloc:iiop:%s:%d/NameService" % (rtm.nshost, rtm.nsport)
rtm.rootnc = rtm.orb.string_to_object(nsloc)._narrow(
CosNaming.NamingContext)
try:
import cnoid.Corba
rtm.orb = cnoid.Corba.getORB()
except:
rtm.initCORBA()
try:
setNameserver()
camera = rtm.findRTC("V4L2MultiCamera0")
if camera == None:
raise Exception("Failed to find V4L2MultiCamera0")
viewer = rtm.findRTC("MultiImageViewer0")
if viewer == None:
raise Exception("Failed to find MultiImageViewer0")
cpanel = rtm.findRTC("ControlPanel0")
if cpanel == None:
raise Exception("Failed to find ControlPanel0")
connectPorts(camera.port("TimedImages"), viewer.port("images"))
connectPorts(cpanel.port("Command"), camera.port("Command"))
camera.start()
viewer.start()
cpanel.start()