options = yarp.Property()
options.put('device','CartesianControlClient')
options.put('cartesianRemote','/teo/leftArm/CartesianControl')
options.put('cartesianLocal','/cc/teo/leftArm')
ddLeft = yarp.PolyDriver(options)
options.put('cartesianRemote','/teo/rightArm/CartesianControl')
options.put('cartesianLocal','/cc/teo/rightArm')
ddRight = yarp.PolyDriver(options)
iccLeft = kinematics_dynamics.viewICartesianControl(ddLeft)
iccRight = kinematics_dynamics.viewICartesianControl(ddRight)
xLeft = yarp.DVector()
xRight = yarp.DVector()
# Se calcula la FK (cinematica directa)
# 6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians)
iccLeft.stat(xLeft)
iccRight.stat(xRight)
# Esta matriz representa la matriz homogenea 4x4 que va de 0 a N (0~origen, N~gripper)
H_left_0_N = np.eye(4)
H_right_0_N = np.eye(4)
# https://stackoverflow.com/a/25709323
# axis2dcm convierte: scaled axis-angle representation (radians) -> dcm (matriz de cosenos directores)
axis2dcm = lambda(axis): lin.expm3(np.cross(np.eye(3), axis))
quit()
pos = dd.viewIPositionControl(
) # make a position controller object we call 'pos'
vel = dd.viewIVelocityControl(
) # make a velocity controller object we call 'vel'
posd = dd.viewIPositionDirect(
) # make a direct position controller object we call 'posd'
enc = dd.viewIEncoders() # make an encoder controller object we call 'enc'
mode = dd.viewIControlMode(
) # make a operation mode controller object we call 'mode'
ll = dd.viewIControlLimits() # make a limits controller object we call 'll'
axes = enc.getAxes() # retrieve number of joints
lmin = yarp.DVector(1)
lmax = yarp.DVector(1)
ll.getLimits(0, lmin, lmax) # retrieve limits of joint 0
print('lmin', lmin[0], 'lmax', lmax[0])
# use the object to set the device to position mode (as opposed to velocity mode)(note: stops the robot)
mode.setControlModes(yarp.IVector(axes, yarp.encode('pos')))
print 'positionMove(1,-35) -> moves motor 1 (start count at motor 0) to -35 degrees'
pos.positionMove(1, -35)
done = False
while not done:
print 'wait to reach...'
yarp.delay(1.0) # [s]
done = pos.checkMotionDone()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
# Configure acceleration
for joint in range(0, axesRA):
posRA.setRefAcceleration(
joint, 20) # manual por rpc --> set accs (20 20 20 20 20 20 20 0)
posLA.setRefAcceleration(joint, 20)
# Configure speed
sp = yarp.DVector(axesLA, 30)
posRA.setRefSpeeds(sp)
posLA.setRefSpeeds(sp)
# Configure speech
ttsLang('mb-es1')
# Home
head = yarp.DVector(axesH, 0.0)
ra = yarp.DVector(axesRA, 0.0)
la = yarp.DVector(axesLA, 0.0)
# ------ Script ------
# Right Arm moving
def configure(self, rf):
self.config = yarp.Property()
self.config.fromConfigFile(
'/code/icub_perceptual_optimisation/yarp/config.ini')
self.width = self.config.findGroup('CAMERA').find('width').asInt32()
self.height = self.config.findGroup('CAMERA').find('height').asInt32()
if self.config.findGroup('GENERAL').find('show_images').asBool():
import matplotlib
matplotlib.use('TKAgg')
self.ax_left = plt.subplot(1, 2, 1)
self.ax_right = plt.subplot(1, 2, 2)
# prepare motor driver
self.head_motors = 'head'
self.head_motorprops = yarp.Property()
self.head_motorprops.put("device", "remote_controlboard")
self.head_motorprops.put("local",
"/client_babbling/" + self.head_motors)
self.head_motorprops.put("remote", "/icubSim/" + self.head_motors)
self.left_motors = 'left_arm'
self.left_motorprops = yarp.Property()
self.left_motorprops.put("device", "remote_controlboard")
self.left_motorprops.put("local",
"/client_babbling/" + self.left_motors)
self.left_motorprops.put("remote", "/icubSim/" + self.left_motors)
self.right_motors = 'right_arm'
self.right_motorprops = yarp.Property()
self.right_motorprops.put("device", "remote_controlboard")
self.right_motorprops.put("local",
"/client_babbling/" + self.right_motors)
self.right_motorprops.put("remote", "/icubSim/" + self.right_motors)
# create remote driver
self.head_driver = yarp.PolyDriver(self.head_motorprops)
print('head motor driver prepared')
# query motor control interfaces
self.head_iPos = self.head_driver.viewIPositionControl()
self.head_iVel = self.head_driver.viewIVelocityControl()
self.head_iEnc = self.head_driver.viewIEncoders()
self.head_iCtlLim = self.head_driver.viewIControlLimits()
self.left_armDriver = yarp.PolyDriver(self.left_motorprops)
print('left motor driver prepared')
# query motor control interfaces
self.left_iPos = self.left_armDriver.viewIPositionControl()
self.left_iVel = self.left_armDriver.viewIVelocityControl()
self.left_iEnc = self.left_armDriver.viewIEncoders()
self.left_iCtlLim = self.left_armDriver.viewIControlLimits()
self.right_armDriver = yarp.PolyDriver(self.right_motorprops)
print('right motor driver prepared')
# query motor control interfaces
self.right_iPos = self.right_armDriver.viewIPositionControl()
self.right_iVel = self.right_armDriver.viewIVelocityControl()
self.right_iEnc = self.right_armDriver.viewIEncoders()
self.right_iCtlLim = self.right_armDriver.viewIControlLimits()
# number of joints
self.num_joints = self.left_iPos.getAxes()
print('Num joints: ', self.num_joints)
self.head_num_joints = self.head_iPos.getAxes()
print('Num head joints: ', self.head_num_joints)
self.head_limits = []
for i in range(self.head_num_joints):
head_min = yarp.DVector(1)
head_max = yarp.DVector(1)
self.head_iCtlLim.getLimits(i, head_min, head_max)
print('lim head ', i, ' ', head_min[0], ' ', head_max[0])
self.head_limits.append([head_min[0], head_max[0]])
self.left_limits = []
self.right_limits = []
for i in range(self.num_joints):
left_min = yarp.DVector(1)
left_max = yarp.DVector(1)
self.left_iCtlLim.getLimits(i, left_min, left_max)
#print('lim left ', i, ' ', left_min[0], ' ', left_max[0])
self.left_limits.append([left_min[0], left_max[0]])
right_min = yarp.DVector(1)
right_max = yarp.DVector(1)
self.right_iCtlLim.getLimits(i, right_min, right_max)
#print('lim right ', i, ' ', right_min[0], ' ', right_max[0])
self.right_limits.append([right_min[0], right_max[0]])
self.go_to_starting_pos()
moduleName = rf.check("name", yarp.Value("BabblingModule")).asString()
self.setName(moduleName)
print('module name: ', moduleName)
yarp.delay(5.0)
print('starting now')
options = yarp.Property()
options.put('device', 'CartesianControlClient')
options.put('cartesianRemote', '/teoSim/rightArm/CartesianControl')
options.put('cartesianLocal', '/cartesianControlExample')
options.put('transform', 1)
dd = yarp.PolyDriver(options) # calls open -> connects
if not dd.isValid():
print 'Cannot open the device!'
raise SystemExit
cartesianControl = kinematics_dynamics.viewICartesianControl(
dd) # view the actual interface
print '> stat'
x = yarp.DVector()
ret, state, ts = cartesianControl.stat(x)
print '<', yarp.Vocab.decode(state), '[%s]' % ', '.join(map(str, x))
xd = [0, 0, 0, 0, 0, 0, 0]
xd[0] = [0.389496, -0.34692, 0.16769, 1.0, 0.0, 0.0, 0.0]
xd[1] = [0.5, -0.34692, 0.16769, 1.0, 0.0, 0.0, 0.0]
xd[2] = [0.5, -0.4, 0.16769, 1.0, 0.0, 0.0, 0.0]
xd[3] = [0.5, -0.4, 0.16769, 0.0, 1.0, 0.0, -12.0]
xd[4] = [0.5, -0.4, 0.16769, 1.0, 0.0, 0.0, -50.0]
xd[5] = [0.389496, -0.34692, 0.16769, 1.0, 0.0, 0.0, 0.0]
xd[6] = [0.0, -0.34692, -0.221806, 0.0, 1.0, 0.0, 90.0]
for i in range(len(xd)):
print '-- movement ' + str(i + 1) + ':'
def moveTrunk(axial, frontal):
tr = yarp.DVector(axesTR, 0.0)
tr = list([axial, frontal])
posTR.positionMove(yarp.DVector(tr))
while not posTR.checkMotionDone():
sleep(0.1)
) # make a position controller object we call 'pos'
encSN = ddSN.viewIEncoders() # encoders
axesSN = posSN.getAxes() # retrieve number of joints
#create a new input port and open it
in_port = yarp.Port()
in_port.open("/softimu/in")
#connect up the output port to our input port
yarp.Network.connect("/softimu/out", "/softimu/in")
# Configure acceleration
for joint in range(0, axesTR):
posTR.setRefAcceleration(joint, 10) # manual por rpc --> set accs (20 20)
# Configure speed
sp = yarp.DVector(axesTR, 10)
posTR.setRefSpeeds(sp)
# writing CSV
start = yarp.now()
imu = yarp.Vector(2)
def writePossToCsv():
with open(csvFile, 'w') as csvOutfile:
csvwriter = csv.writer(csvOutfile, delimiter=',')
csvwriter.writerow([
'timestamp', 'axial-trunk', 'frontal-trunk', 'neck-m1', 'neck-m2',
'neck-m3', 'imu-roll', 'imu-pitch'
])
while (1):
# trunkRightArmIControlLimits = trunkRightArmDevice.viewIControlLimits()
# if trunkRightArmIControlLimits == []:
# print("Right arm control limits interface NOT available")
# else:
# print("Right arm control limits interface available.")
rightArmIControlLimits = rightArmDevice.viewIControlLimits()
if rightArmIControlLimits == []:
print("Right arm control limits interface NOT available")
else:
print("Right arm control limits interface available.")
qrMin = yarp.Bottle()
qrMax = yarp.Bottle()
lmin = yarp.DVector(numRightArmJoints)
lmax = yarp.DVector(numRightArmJoints)
for joint in range(numRightArmJoints):
rightArmIControlLimits.getLimits(joint, lmin, lmax)
qrMin.addDouble(float(lmin[0]))
qrMax.addDouble(float(lmax[0]))
print("Joint ", joint, " min: ", lmin[0], "max: ", lmax[0])
print(qrMin.toString())
limits = "(mins (" + qrMin.toString() + ")) (maxs (" + qrMax.toString() + "))"
trunkRightArmSolverOptions = yarp.Property()
trunkRightKinPath = rf.findFileByName("teo-trunk-rightArm-fetch.ini")
trunkRightArmSolverOptions.fromConfigFile(trunkRightKinPath)
trunkRightArmSolverOptions.put("device", "KdlSolver")
print('Unable to connect to remote sync port')
raise SystemExit
if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
print('Unable to set position direct mode')
raise SystemExit
time.sleep(0.1) # hacky, but we need to make sure remote data arrived prior to calling getEncoder()
initialPos = enc.getEncoder(args.joint)
print('Current enc value: %d' % initialPos)
input('Press ENTER to start')
print('Moving joint %d to %d degrees...' % (args.joint, args.target))
distance = args.target - initialPos
velocity = math.copysign(args.speed, distance)
callback = SyncCallback(initialPos, velocity, lambda pos: posd.setPosition(args.joint, pos))
syncPort.useCallback(callback)
lastRef = yarp.DVector(1)
while posd.getRefPosition(args.joint, lastRef) and abs(lastRef[0] - initialPos) < abs(distance):
time.sleep(0.01)
syncPort.interrupt()
syncPort.close()
dd.close()
yarp.Network.fini()
def configure(self, rf):
self.config = yarp.Property()
self.config.fromConfigFile('/code/icub_intrinsic_motivation/yarp/config.ini')
self.width = self.config.findGroup('CAMERA').find('width').asInt32()
self.height = self.config.findGroup('CAMERA').find('height').asInt32()
if self.config.findGroup('GENERAL').find('show_images').asBool():
import matplotlib
matplotlib.use('TKAgg')
self.ax_left = plt.subplot(1, 2, 1)
self.ax_right = plt.subplot(1, 2, 2)
# Create a port and connect it to the iCub simulator virtual camera
self.input_port_cam = yarp.Port()
self.input_port_cam.open("/icub/camera_left")
yarp.Network.connect("/icubSim/cam/left", "/icub/camera_left")
# prepare image
self.yarp_img_in = yarp.ImageRgb()
self.yarp_img_in.resize(self.width, self.height)
self.img_array = np.ones((self.height, self.width, 3), dtype=np.uint8)
# yarp image will be available in self.img_array
self.yarp_img_in.setExternal(self.img_array.data, self.width, self.height)
# prepare motor driver
self.left_motors = 'left_arm'
self.left_motorprops = yarp.Property()
self.left_motorprops.put("device", "remote_controlboard")
self.left_motorprops.put("local", "/client/" + self.left_motors)
self.left_motorprops.put("remote", "/icubSim/" + self.left_motors)
self.right_motors = 'right_arm'
self.right_motorprops = yarp.Property()
self.right_motorprops.put("device", "remote_controlboard")
self.right_motorprops.put("local", "/client/" + self.right_motors)
self.right_motorprops.put("remote", "/icubSim/" + self.right_motors)
# create remote driver
self.left_armDriver = yarp.PolyDriver(self.left_motorprops)
print('left motor driver prepared')
# query motor control interfaces
self.left_iPos = self.left_armDriver.viewIPositionControl()
self.left_iVel = self.left_armDriver.viewIVelocityControl()
self.left_iEnc = self.left_armDriver.viewIEncoders()
self.left_iCtlLim = self.left_armDriver.viewIControlLimits()
self.right_armDriver = yarp.PolyDriver(self.right_motorprops)
print('right motor driver prepared')
# query motor control interfaces
self.right_iPos = self.right_armDriver.viewIPositionControl()
self.right_iVel = self.right_armDriver.viewIVelocityControl()
self.right_iEnc = self.right_armDriver.viewIEncoders()
self.right_iCtlLim = self.right_armDriver.viewIControlLimits()
# number of joints
self.num_joints = self.left_iPos.getAxes()
print('Num joints: ', self.num_joints)
self.left_limits = []
self.right_limits = []
for i in range(self.num_joints):
left_min =yarp.DVector(1)
left_max =yarp.DVector(1)
self.left_iCtlLim.getLimits(i, left_min, left_max)
print('lim left ', i, ' ', left_min[0], ' ', left_max[0])
self.left_limits.append([left_min[0], left_max[0]])
right_min =yarp.DVector(1)
right_max =yarp.DVector(1)
self.right_iCtlLim.getLimits(i, right_min, right_max)
print('lim right ', i, ' ', right_min[0], ' ', right_max[0])
self.right_limits.append([right_min[0], right_max[0]])
self.go_to_starting_pos()
moduleName = rf.check("name", yarp.Value("BabblingModule")).asString()
self.setName(moduleName)
print('module name: ',moduleName)
yarp.delay(1.0)
print('starting now')