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
def executePose(self, pose):
#set positions and wait that the actuator reaching that position
#TODO tener en cuenta los motores que están invertidos, creo que los únicos que están quedando son los de los hombros
for joint in self.RobotImplementedJoints:
angle = pose.getValue(joint)
angleAX = AXAngle()
angleAX.setDegreeValue(angle)
#TODO implement method for setting position of all actuators at the same time
self.actuator.move_actuator(self.robotConfiguration.loadJointId(joint), int(angleAX.getValue()), self.defaultSpeed)
self.poseExecuted = self.poseExecuted + 1
print "pose executed!"
time.sleep(0.1)
def executePose(self, pose):
# set positions and wait that the actuator reaching that position
dontSupportedJoints = self.sysConf.getVrepNotImplementedBioloidJoints()
RobotImplementedJoints = []
for joint in self.joints:
if joint not in dontSupportedJoints:
RobotImplementedJoints.append(joint)
for joint in RobotImplementedJoints:
angle = pose.getValue(joint)
angleAX = AXAngle()
angleAX.setDegreeValue(angle)
# TODO implement method for setting position of all actuators at the same time
self.actuator.move_actuator(
self.robotConfiguration.loadJointId(joint), int(angleAX.getValue()), self.defaultSpeed
)
self.poseExecuted = self.poseExecuted + 1
time.sleep(3)
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)
