def __init__(self, arm_portbasename, handlername="HandlerArmJoint"):
prename = arm_portbasename
full_name = prename + "/" + handlername
self.outp = yarp.BufferedPortBottle()
self.inp = yarp.BufferedPortBottle()
outp_name = full_name + "/to_js"
inp_name = full_name + "/q"
self.outp.open(outp_name)
self.inp.open(inp_name)
yarp_connect_blocking(outp_name, prename + "/jpctrl/ref")
yarp_connect_blocking(prename + "/bridge/encoders", inp_name)
def __init__(self,
arm_portbasename,
handlername="HandlerArmBridge",
torso=True):
self.torso = torso
prename = arm_portbasename
full_name = prename + "/" + handlername
self.outp = yarp.BufferedPortBottle()
outp_name = full_name + "/toBridge_weights"
self.outp.open(outp_name)
if self.torso:
self.torso_port = yarp.BufferedPortBottle()
torso_port_name = full_name + "/to_torso_cjoints"
self.torso_port.open(torso_port_name)
yarp_connect_blocking(torso_port_name,
prename + "/bridge/torso_cjoints:i")
self.VFW_port = yarp.BufferedPortBottle()
VFW_port_name = full_name + "/to_VF_weight:o"
self.VFW_port.open(VFW_port_name)
self.encoders_port = yarp.BufferedPortBottle()
encoders_port_name = full_name + "/encoders:i"
self.encoders_port.open(encoders_port_name)
yarp_connect_blocking(outp_name, prename + "/bridge/weights")
yarp_connect_blocking(VFW_port_name, prename + "/vectorField/weight")
yarp_connect_blocking(prename + "/bridge/encoders", encoders_port_name)
def __init__(self,arm_portbasename, handlername="/HandlerArm"):
import imp
prename=arm_portbasename
full_name=prename+ handlername
self.outp=yarp.BufferedPortBottle()
outp_name=full_name +"/toObjectFeeder"
self.outp.open(outp_name)
self.stiffness_port=yarp.BufferedPortBottle()
stiffness_port_name=full_name +"/stiffness"
self.stiffness_port.open(stiffness_port_name)
self.goaldistp=yarp.BufferedPortBottle()
goaldistp_name=full_name + "/fromGoalDistance"
self.goaldistp.open(goaldistp_name)
#self.goaldistp.setStrict()
self.posep=yarp.BufferedPortBottle()
posep_name=full_name+"/pose:i"
self.posep.open(posep_name)
yarp_connect_blocking(outp_name,prename+"/ofeeder/object")
yarp_connect_blocking(stiffness_port_name,prename+"/robot/stiffness")
yarp_connect_blocking(prename+"/dmonitor/distOut" , goaldistp_name)
yarp_connect_blocking(prename+"/vectorField/pose", posep_name)
self.current_frame=[1.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
self.current_slowdown_distance=0.1
self.toolp = new_port(full_name + "/toToolin", 'out', "%s/vectorField/tool"%(prename))
self.toolp.setStrict(True)
self.goal_threshold=0.01
