def executePose(self, pose):
error = False
#Above's N joints will be received and set on the V-REP side at the same time
jointsQty = len(self.RobotImplementedJoints)
jointExecutedCounter = 0
error = self.sim.pauseSim(self.clientID) or error
for joint in self.RobotImplementedJoints:
angle = pose.getValue(joint)
angleAX = AXAngle()
angleAX.setDegreeValue(angle)
#print "setting joint: ", joint, " to value: ", angle
if jointExecutedCounter < jointsQty - 1:
error = self.sim.setJointPositionNonBlock(self.clientID, joint, angleAX.toVrep()) or error
else:
###self.moveHelperArm()
error = self.sim.resumePauseSim(self.clientID) or error
error = self.sim.setJointPosition(self.clientID, joint, angleAX.toVrep()) or error
jointExecutedCounter += 1
self.updateLucyPosition()
self.poseExecuted = self.poseExecuted + self.getPosesExecutedByStepQty()
def executePose(self, pose):
error = False
dontSupportedJoints = self.sysConf.getVrepNotImplementedBioloidJoints()
RobotImplementedJoints = []
# Above's N joints will be received and set on the V-REP side at the same time
# TODO this must be checked in the simulator class
if self.jointHandleCachePopulated == False:
self.sim.populateJointHandleCache(self.clientID)
self.jointHandleCachePopulated = True
error = self.sim.pauseSim(self.clientID) or error
for joint in self.joints:
if joint not in dontSupportedJoints:
RobotImplementedJoints.append(joint)
jointsQty = len(RobotImplementedJoints)
jointExecutedCounter = 0
for joint in RobotImplementedJoints:
angle = pose.getValue(joint)
angleAX = AXAngle()
angleAX.setDegreeValue(angle)
if jointExecutedCounter < jointsQty - 1:
error = self.sim.setJointPositionNonBlock(self.clientID, joint, angleAX.toVrep()) or error
else:
error = self.sim.resumePauseSim(self.clientID) or error
error = self.sim.setJointPosition(self.clientID, joint, angleAX.toVrep()) or error
jointExecutedCounter = jointExecutedCounter + 1
self.updateLucyPosition()
self.poseExecuted = self.poseExecuted + 1
standadRemoteApiPort=19997
localhost='127.0.0.1'
genetic_bioloid=os.getcwd()+"/models/genetic_bioloid.ttt"
print 'Program started'
angle = AXAngle()
sim = Simulator()
clientID = sim.connectVREP()
if clientID !=-1:
print 'Connected to remote API server'
sim.loadscn(clientID, genetic_bioloid)
sim.startSim(clientID,True)
angle.setDegreeValue(150)
sim.setJointPosition(clientID,"R_Hip_Pitch",angle.toVrep())
sim.setJointPosition(clientID,"L_Hip_Pitch",angle.toVrep())
pos1x, pos1y=sim.getBioloidPlannarPosition(clientID)
end=False
while not end:
for i in xrange(30):
angle.setDegreeValue(150-i)
#print "voy a setear ax12 value en cadera izquierda: ", 150 - i
sim.setJointPosition(clientID,'L_Hip_Pitch',angle.toVrep())
angle.setDegreeValue(i*2)
#print "voy a setear ax12 value en rodilla derecha: ", i
sim.setJointPosition(clientID,'R_Knee',angle.toVrep())
for i in xrange(30):
angle.setDegreeValue(150-i)
sim.setJointPosition(clientID,'R_Hip_Pitch',angle.toVrep())
# angle.setValue(value)
# setJointPosition(clientID,"L_Hip_Pitch",angle.toVrep())
#angle.setDegreeValue(210)
#end=False
#error=1
#while not end:
# error, up=isRobotUp(clientID)
# end=not up
#print "me cai"
pos1x, pos1y=getBioloidPlannarPosition(clientID)
for i in range(25):
angle.setDegreeValue(150-i)
print "voy a setear ax12 value en cadera izquierda: ", 150 - i
setJointPosition(clientID,"L_Hip_Pitch",angle.toVrep())
angle.setDegreeValue(i*2)
print "voy a setear ax12 value en rodilla derecha: ", i
setJointPosition(clientID,"R_Knee",angle.toVrep())
#angle.setDegreeValue(150)
#setJointPosition(clientID,"R_Hip_Pitch",angle.toVrep())
print "termine"
time.sleep(5)
for i in range(30):
angle.setDegreeValue(150-i)
setJointPosition(clientID,"R_Hip_Pitch",angle.toVrep())
setJointPosition(clientID,"L_Knee",angle.toVrep())
isRobotUp(clientID)
isRobotUp(clientID)
