def __init__(self, worldfn): GLRealtimeProgram.__init__(self, "Hubo-ach virtual robot server") # Create simulator self.simulator = MakeSimulator(worldfn) # Hubo-Ach Start and setup: self.stateChannel = ach.Channel(ha.HUBO_CHAN_STATE_NAME) self.stateChannel.flush() self.state = ha.HUBO_STATE() self.state.time = 0 self.refChannel = ach.Channel(ha.HUBO_CHAN_REF_NAME) self.refChannel.flush() self.ref = ha.HUBO_REF() self.prevRef = ha.HUBO_REF() #self.timeChannel = ach.Channel(ha.HUBO_CHAN_VIRTUAL_TO_SIM_NAME) #self.timeChannel.flush() self.sim = ha.HUBO_VIRTUAL() self.feedbackChannel = ach.Channel(ha.HUBO_CHAN_VIRTUAL_FROM_SIM_NAME) self.feedbackChannel.flush()
def __init__(self): self.state = ha.HUBO_STATE() self.ref = ha.HUBO_REF() self.stateChan = ach.Channel(ha.HUBO_CHAN_STATE_NAME) self.refChan = ach.Channel(ha.HUBO_CHAN_REF_NAME) self.lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1") self.stateChan.flush() self.refChan.flush() self.subscription = self.lc.subscribe("HuboInput", self.command_handler)
def __init__(self , constraintsAngles , numberOfSteps , accuracy = 0.01): self.initTime = 0.0 bendKneesNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR']) bendKneesValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 , 0. , 0. , 0. , 0.],dtype=float) straightKneesNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR']) straightKneesValues = np.array([0. , 0. , 0., 0. , 0. , 0. ],dtype=float) shiftWeightNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' ,'RHR' , 'LHR' , 'RAR' , 'LAR']) shiftWeightLeftValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 ,-0.28 , -0.28 , 0.28 , 0.28],dtype=float) shiftWeightCenterValues = np.array([0.,0.,0.,0.],dtype=float) shiftWeightRightValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 ,.28 , 0.28 , -0.28 , -0.28],dtype=float) kneePunchNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR']) kneePunchRightValues = np.array([1.4 , 1.3 , -1. , -.83 , -0.6 , -0.6 , -.28 , -.28 , .28 , .28],dtype=float) kneePunchLeftValues = np.array([1.3 , 1.4 , -.83 , -.9 , -0.6 , -0.6 , .28 , .28 , -.28 , -.28],dtype=float) landLegNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR']) landRightLegValues = np.array([1.3017 , 1.3 , -.269 , -0.83 ,0.,-0.6,-.28,-.28,0,0.]) landLeftLegValues = np.array([1.3 , 1.3017 , -0.83 , -.269 , -0.6 , 0.,.28,.28, 0.,.0]) shiftWeightAgainNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR']) shiftWeightLeftToRightValues = np.array([1.3017 , 1.899 , -.269 , -.6225 , 0. , 0. , 0. , 0. , 0. , 0. ],dtype=float) shiftWeightRightToLeftValues = np.array([1.3017 , 1.899 , -.269 , -.6225 , 0. , 0. , 0. , 0. , 0. , 0. ],dtype=float) s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) state = ha.HUBO_STATE() ref = ha.HUBO_REF() sim = ha.HUBO_VIRTUAL() [status , framesize] = s.get(state,wait=False,last=True) current = np.zeros([np.size(shiftWeightNames),1]) gain = 0 print 'bend' current = self._sendCommand(r , ref , bendKneesNames , bendKneesValues,current , gain , correct=False) print 'shift' current = self._sendCommand(r , ref , shiftWeightNames , shiftWeightLeftValues,current , gain , correct=True) print 'kneePunch' current = self._sendCommand(r , ref , kneePunchNames , kneePunchRightValues,current , gain , correct=True) current = self._sendCommand(r, ref, landLegNames, landRightLegValues, current, gain, correct=True) current = self._sendCommand(r , ref, shiftWeightAgainNames, shiftWeightLeftToRightValues, current, gain, correct=True) simTime = ha.HUBO_LOOP_PERIOD
def __init__(self): low = .5 high = .78285 increment = 15 lowerLeg = .30038 upperLeg = .30003 torso = .18247 out = ik2DOF(low, 0., lowerLeg, upperLeg) theta1 = out.theta1 theta2 = out.theta2 theta3 = 0. - theta1 - theta2 # print theta1 # print theta2 s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) state = ha.HUBO_STATE() ref = ha.HUBO_REF() # sim = ha.HUBO_VIRTUAL() [status, framesize] = s.get(state, wait=False, last=True) currentValues = self._returnDOFValues(state, s) OGTheta1 = currentValues[17] OGTheta2 = currentValues[16] OGTheta3 = currentValues[15] stillGoing = True workingInc = 0 while stillGoing: self._sendCommand(s, r, ref, state, theta1, theta2, theta3, increment, 'DOWN') self.simulationDelay(1, state, s) self._sendCommand(s, r, ref, state, OGTheta1, OGTheta2, OGTheta3, increment, 'UP') self.simulationDelay(2, state, s) workingInc += 1 if workingInc > 4: stillGoing = False
def __init__(self): ### Standard Intro ### s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) state = ha.HUBO_STATE() ref = ha.HUBO_REF() [status , framesize] = s.get(state,wait=False,last=True) self.rightCoordinateNames = np.array(['RSP','RSR' ,'RSY','REB','RWR','RWP']) self.leftCoordinateNames = np.array(['LSP','LSR' ,'LSY','LEB','LWR','LWP']) sizes = np.array([]) deltaTheta = .1 error = 5 step = 10 leftGoal = np.array([]) rightGoal = np.array([])
) import baxter_interface from baxter_interface import CHECK_VERSION # Hubo-ach stuff import hubo_ach as ha import ach from ctypes import * import time # feed-forward will now be refered to as "state" state = ha.HUBO_STATE() # feed-back will now be refered to as "ref" ref = ha.HUBO_REF() # Get the current feed-forward (state) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) [statuss, framesizes] = r.get(ref, wait=False, last=True) class GripperClient(object): def __init__(self, gripper): ns = 'robot/end_effector/' + gripper + '_gripper/' self._client = actionlib.SimpleActionClient( ns + "gripper_action", GripperCommandAction, ) self._goal = GripperCommandGoal() # Wait 10 Seconds for the gripper action server to start or exit
def __init__(self): ### Standard Intro ### s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) state = ha.HUBO_STATE() ref = ha.HUBO_REF() [status, framesize] = s.get(state, wait=False, last=True) self.rightCoordinateNames = np.array( ['RSP', 'RSR', 'RSY', 'REB', 'RWY', 'RWP']) self.leftCoordinateNames = np.array( ['LSP', 'LSR', 'LSY', 'LEB', 'LWY', 'LWP']) self.sizes = np.array([[300.38 + 300.03 + 303.87 - 181.87], [179.14], [181.59]]) deltaTheta = .1 error = 5 step = 30 print "\n\n\nWELCOME TO INVERSE KINEMATICS" print "CENTERING ARMS AND CALCULATION BOX BASED ON THIS POSITION.\n" print "THIS SIMULATION RUNS SLOW BECAUSE IT IS DOING THE PLANNING ONLINE" print "INSTEAD OF DOING IT A PRIORI.\n" print "THIS MEANS IT IS TREATING THE NATURAL PHYSICS AS A DISTURBANCE" print "WHICH MAKES THE CONTROL SLOWER." print "IF THE CONTROL IS NOT SET CORRECTLY THEN SINGULARITIES ARE FOUND" print "AND THINGS LIKE ARMS GO BACKWARDS AROUND IT TO TRY AND SOLVE THE STEP." print "\n\nENJOY..." self._raiseArmsToCenter(state, s, r, ref) self._simulationDelay(1, state, s) left = self._getFK(0, False, 'L', state, s) right = self._getFK(0, False, 'R', state, s) leftGoal = np.array([[left[0] + 50., left[1], left[2] + 60.], [left[0] + 50., left[1], left[2] - 60.], [left[0] - 50., left[1], left[2] - 60.], [left[0] - 50., left[1], left[2] + 60.]]) rightGoal = np.array([[right[0] + 50., right[1], right[2] + 50.], [right[0] + 50., right[1], right[2] - 50.], [right[0] - 50., right[1], right[2] - 50.], [right[0] - 50., right[1], right[2] + 50.]]) run = True runningCount = 0 goal = 0 print "RUNNING ALGORITHM FOR 4 GOALS" while run: leftError = self._error(leftGoal[goal, ], 'L', state, s) rightError = self._error(rightGoal[goal, ], 'R', state, s) print "\n\nGOAL NUMBER {} \n\n".format(goal) print "LEFT HAND GOAL POSITION IS: {}".format(leftGoal[goal, ]) print "RIGHT HAND GOAL POSITION IS: {}".format(rightGoal[goal, ]) print "\n\n" print '\tLeft Error = {} , Right Error = {}'.format( round(leftError, 3), round(rightError, 3)) while leftError > error or rightError > error: if runningCount == 50: print '\tLeft Error = {} , Right Error = {}'.format( round(leftError, 3), round(rightError, 3)) runningCount = 0 lSideJacobian = self._getJacobian('L', deltaTheta, state, s) rSideJacobian = self._getJacobian('R', deltaTheta, state, s) lSideJacobian = np.linalg.pinv(lSideJacobian) rSideJacobian = np.linalg.pinv(rSideJacobian) leftNextStep = self._nextStep(step, leftError, state, s, 'L', leftGoal[goal, ]) rightNextStep = self._nextStep(step, rightError, state, s, 'R', rightGoal[goal, ]) lThetaUpdate = np.dot(lSideJacobian, leftNextStep) rThetaUpdate = np.dot(rSideJacobian, rightNextStep) self._adjustArms(state, s, r, ref, lThetaUpdate, rThetaUpdate) leftError = self._error(leftGoal[goal, ], 'L', state, s) rightError = self._error(rightGoal[goal, ], 'R', state, s) # self._simulationDelay(.005,state,s) runningCount += 1 goal += 1 if goal == 4: run = False print "\n\n\tFIN\n\n"
def __init__(self): self.rightCoordinateNames = np.array(['RSP', 'RSR' ]) #,'RSY','REB','RWR','RWP']) self.leftCoordinateNames = np.array(['LSP', 'LSR' ]) #,'LSY','LEB','LWR','LWP']) self.rightGoal = np.array([[0., 0., -360.73], [-138.05, -127.54, -307.90], [138.05, 127.54, -307.90], [127.54, 52.83, -333.27], [-37.35, -270.55, -235.66]]) self.leftGoal = np.array([[0., 0., -360.73], [-138.05, -127.54, -307.90], [138.05, 127.54, -307.90], [127.54, 52.83, -333.27], [-37.35, -270.55, -235.66]]) self.dhArrayRight = np.array([ [0, -np.pi / 2., 0., 0.], # RSP [0., 0., 179.14 + 181.59, 0.] ]) # RSR self.dhArrayLeft = np.array([ [0., -np.pi / 2, 0., 0.], # LSP [0., 0., 179.14 + 181.59, 0.] ]) # LSR deltaTheta = .1 deltaError = np.array([.1, .1, .1]) s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) state = ha.HUBO_STATE() ref = ha.HUBO_REF() [status, framesize] = s.get(state, wait=False, last=True) # rightCoordinates = np.zeros([1,np.size(self.rightCoordinateNames)]) # leftCoordinates = np.zeros([1,np.size(self.leftCoordinateNames)]) errorOk = .01 ##### MAIN ALGORITHM HERE ##### running = True goal = 0 while running: self._getCurrentTheta(state, s) # Get Current Location rightError = self._getMet('R', self.rightGoal, goal) leftError = self._getMet('L', self.leftGoal, goal) while rightError > errorOk and leftError > errorOk: rightJacobian = getJacobian(self.dhArrayRight, deltaTheta).jacobian leftJacobian = getJacobian(self.dhArrayLeft, deltaTheta).jacobian rightJacobianInverse = np.linalg.pinv(rightJacobian) leftJacobianInverse = np.linalg.pinv(leftJacobian) rightCurrent = self._getFK(self.dhArrayRight) leftCurrent = self._getFK(self.dhArrayLeft) rightDeltaError = self._getNext(rightCurrent, self.rightGoal, deltaError, 'R', goal) leftDeltaError = self._getNext(leftCurrent, self.leftGoal, deltaError, 'L', goal) # print np.shape(rightDeltaError) , np.shape(rightJacobianInverse) # print rightJacobianInverse rightDeltaTheta = np.squeeze( np.dot(rightDeltaError, rightJacobianInverse)) leftDeltaTheta = np.squeeze( np.dot(leftDeltaError, leftJacobianInverse)) # for i in range(np.shape(self.dhArrayRight)[0]): # self.dhArrayRight[i,0] += rightDeltaTheta[i] # # for i in range(np.shape(self.dhArrayLeft)[0]): # self.dhArrayLeft[i,0] += leftDeltaTheta[i] print rightDeltaTheta print self.dhArrayRight self._sendMoveCommand( s, r, ref, state, rightDeltaTheta + self.dhArrayRight[:, 0], leftDeltaTheta + self.dhArrayLeft[:, 0]) goal += 1 if goal > np.shape(self.rightGoal)[0]: goal = 1
def main(settings): # Open Hubo-Ach feed-forward and feed-back (reference and state) channels s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) e = ach.Channel(ha.HUBO_CHAN_ENC_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) #s.flush() #r.flush() # feed-forward will now be refered to as "state" state = ha.HUBO_STATE() # encoder channel will be refered to as "encoder" encoder = ha.HUBO_ENC() # feed-back will now be refered to as "ref" ref = ha.HUBO_REF() # Get the current feed-forward (state) [statuss, framesizes] = s.get(state, wait=False, last=True) [statuss, framesizes] = e.get(encoder, wait=False, last=True) portName = settings['port'] baudRate = settings['baudRate'] highestServoId = settings['highestServoId'] # Establish a serial connection to the dynamixel network. # This usually requires a USB2Dynamixel serial = dynamixel.SerialStream(port=portName, baudrate=baudRate, timeout=1) net = dynamixel.DynamixelNetwork(serial) # Ping the range of servos that are attached print "Scanning for Dynamixels..." net.scan(1, highestServoId) myActuators = [] for dyn in net.get_dynamixels(): print dyn.id myActuators.append(net[dyn.id]) if not myActuators: print 'No Dynamixels Found!' sys.exit(0) else: print "...Done" for actuator in myActuators: actuator.moving_speed = 50 actuator.synchronized = True actuator.torque_enable = True actuator.torque_limit = 800 actuator.max_torque = 800 # Randomly vary servo position within a small range print myActuators print "Hubo-Ach neck server active" # print "Servo \tPosition" while True: # Get the current feed-forward (state) [statuss, framesizes] = s.get(state, wait=False, last=True) for actuator in myActuators: if (actuator.id == 1): actuator.goal_position = rad2dyn(state.joint[ha.NKY].ref) if (actuator.id == 2): actuator.goal_position = rad2dyn(state.joint[ha.NK1].ref) if (actuator.id == 3): actuator.goal_position = rad2dyn(state.joint[ha.NK2].ref) net.synchronize() for actuator in myActuators: actuator.read_all() time.sleep(0.01) if (actuator.id == 1): encoder.enc[ha.NKY] = dyn2rad(actuator.current_position) if (actuator.id == 2): encoder.enc[ha.NK1] = dyn2rad(actuator.current_position) if (actuator.id == 3): encoder.enc[ha.NK2] = dyn2rad(actuator.current_position) # print encoder.enc[ha.NKY], " : ", encoder.enc[ha.NK1], " : ", encoder.enc[ha.NK2] e.put(encoder) time.sleep(0.02)