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 get_position(self, id): AXposition = AXAngle() self.communication.flushInput() # to empty the data buffer bytesToRead = 0x02 positionRequestMsg = self.make_msg(id, Instruction.READ_DATA, [Register.CURRENT_POSITION, bytesToRead]) self.communication.send_msg(positionRequestMsg) ret = self.communication.recv_msg() positionHighByte = ret[4] positionHighByte = positionHighByte << 8 positionLowByte = ret[3] position = positionHighByte | positionLowByte AXposition.setValue(position) return AXposition
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)
# You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA from Simulator import Simulator from AXAngle import AXAngle import math import os 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):
def finishSimulation(clientID): errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait) error=errorStop or errorClose errorFinish=vrep.simxFinish(clientID) error=error or errorFinish return error def getBioloidPlannarPosition(clientID): error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait) error, bioloid_position = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming) return bioloid_position[0], bioloid_position[1] print 'Program started' angle = AXAngle() lp = LoadPoses() #clientID = connectVREP(windowsSalaIP,standadRemoteApiPort) clientID = connectVREP() pose=lp.getFramePose(1) for joint in pose.keys(): jointHandleMapping[joint]=0 print joint #clientID = connectVREP(salameIP) if clientID !=-1: print 'Connected to remote API server' res,objs=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait) print objs #loadscn(clientID,genetic_bioloid_salame)