Esempio n. 1
0
    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()
Esempio n. 2
0
    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
Esempio n. 3
0
 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
Esempio n. 4
0
File: Lucy.py Progetto: dtbinh/lucy
    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)
Esempio n. 5
0
 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)
Esempio n. 6
0
# 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)