def main(portName, portBaud): print "INITIALIZING HEAD NODE..." ###Communication with dynamixels: global dynMan1 print "Head.->Trying to open port on " + portName + " at " + str(portBaud) dynMan1 = Dynamixel.DynamixelMan(portName, portBaud) pan = 0 tilt = 0 i = 0 ### Set controller parameters #dynMan1.SetDGain(1, 25) #dynMan1.SetPGain(1, 16) #dynMan1.SetIGain(1, 1) #dynMan1.SetDGain(5, 25) #dynMan1.SetPGain(5, 16) #dynMan1.SetIGain(5, 1) ### Set servos features #dynMan1.SetMaxTorque(1, 1023) dynMan1.SetTorqueLimit(1, 512) #dynMan1.SetHighestLimitTemperature(1, 80) #dynMan1.SetMaxTorque(5, 1023) dynMan1.SetTorqueLimit(5, 512) #dynMan1.SetHighestLimitTemperature(5, 80) ###Connection with ROS rospy.init_node("head") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = ["pan_connect", "tilt_connect"] jointStates.position = [0, 0] ## Subscribers subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead) pubCurrentPose = rospy.Publisher("/hardware/head/current_pose", Float32MultiArray, queue_size=1) #subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size=1) msgCurrentPose = Float32MultiArray() msgCurrentPose.data = [0, 0] dynMan1.SetCWAngleLimit(5, 0) dynMan1.SetCCWAngleLimit(5, 4095) dynMan1.SetGoalPosition(5, 529) dynMan1.SetGoalPosition(1, 512) dynMan1.SetTorqueEnable(5, 1) dynMan1.SetTorqueEnable(1, 1) dynMan1.SetMovingSpeed(5, 100) dynMan1.SetMovingSpeed(1, 100) loop = rospy.Rate(30) lastPan = 0.0 lastTilt = 0.0 while not rospy.is_shutdown(): # Pose in bits panPose = dynMan1.GetPresentPosition(5) tiltPose = dynMan1.GetPresentPosition(1) #print str(panPose) + " " + str(tiltPose) loop.sleep()
def printRegisters(portName1, portBaud1): global dynMan1 dynMan1 = Dynamixel.DynamixelMan(portName1, portBaud1) for i in range(9): dynMan1.GetRegistersValues(i)
def main(portName, portBaud): print "HardwareHead.->INITIALIZING HEAD NODE..." ###Communication with dynamixels: global dynMan1 global goalPan global goalTilt print "HardwareHead.->Trying to open port on " + portName + " at " + str( portBaud) dynMan1 = Dynamixel.DynamixelMan(portName, portBaud) pan = 0 tilt = 0 i = 0 ### Set controller parameters dynMan1.SetDGain(1, 25) dynMan1.SetPGain(1, 16) dynMan1.SetIGain(1, 1) dynMan1.SetDGain(5, 25) dynMan1.SetPGain(5, 16) dynMan1.SetIGain(5, 1) ### Set servos features #dynMan1.SetMaxTorque(1, 1023) dynMan1.SetTorqueLimit(1, 512) #dynMan1.SetHighestLimitTemperature(1, 80) #dynMan1.SetMaxTorque(5, 1023) dynMan1.SetTorqueLimit(5, 512) #dynMan1.SetHighestLimitTemperature(5, 80) ###Connection with ROS rospy.init_node("head") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = ["pan_connect", "tilt_connect"] jointStates.position = [0, 0] ## Subscribers subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead) pubCurrentPose = rospy.Publisher("/hardware/head/current_pose", Float32MultiArray, queue_size=1) #subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size=1) msgCurrentPose = Float32MultiArray() msgCurrentPose.data = [0, 0] dynMan1.SetCWAngleLimit(5, 1023) dynMan1.SetCCWAngleLimit(5, 3069) dynMan1.SetCWAngleLimit(1, 0) dynMan1.SetCCWAngleLimit(1, 4095) dynMan1.SetGoalPosition(5, 1750) dynMan1.SetGoalPosition(1, 3550) dynMan1.SetTorqueEnable(5, 1) dynMan1.SetTorqueEnable(1, 1) dynMan1.SetMovingSpeed(5, 90) dynMan1.SetMovingSpeed(1, 90) loop = rospy.Rate(30) lastPan = 0.0 lastTilt = 0.0 goalPan = 0 goalTilt = 0 speedPan = 0.1 #These values should represent the Dynamixel's moving_speed speedTilt = 0.1 while not rospy.is_shutdown(): # Pose in bits #panPose = dynMan1.GetPresentPosition(5) #tiltPose = dynMan1.GetPresentPosition(1) # Pose in rad #if panPose != None: # pan = (panPose - 1750)*360/4095*3.14159265358979323846/180 # if abs(lastPan-pan) > 0.78539816339: # pan = lastPan #else: # pan = lastPan #if tiltPose != None: # tilt = (tiltPose - 970)*360/4095*3.14159265358979323846/180 # if abs(lastTilt-tilt) > 0.78539816339: # tilt = lastTilt #else: # tilt = lastTilt #SINCE READING IS NOT WORKING, WE ARE FAKING THE REAL SERVO POSE deltaPan = goalPan - pan deltaTilt = goalTilt - tilt if deltaPan > speedPan: deltaPan = speedPan if deltaPan < -speedPan: deltaPan = -speedPan if deltaTilt > speedTilt: deltaTilt = speedTilt if deltaTilt < -speedTilt: deltaTilt = -speedTilt pan += deltaPan tilt += deltaTilt jointStates.header.stamp = rospy.Time.now() jointStates.position[0] = pan jointStates.position[ 1] = -tilt - 0.08 #goes upwards, but to keep a dextereous system, positive tilt should go downwards pubJointStates.publish( jointStates ) #We substract 0.1 to correct an offset error due to the real head position msgCurrentPose.data = [pan, tilt] pubCurrentPose.publish(msgCurrentPose) if i == 10: msgBatery = float(dynMan1.GetPresentVoltage(5) / 10.0) pubBatery.publish(msgBatery) i = 0 i += 1 lastPan = pan lastTilt = tilt loop.sleep()
def main(portName1, portBaud1): print "JustinaHardwareRightArm.->INITIALIZING RIGHT ARM NODE BY MARCOSOFT..." global objOnHand global speedGripper global torqueGripper global gripperGoal_1 global gripperGoal_2 global torqueGripperCCW1 global torqueGripperCCW2 global gripperTorqueLimit global gripperTorqueActive global goalPos global torqueMode global speedsGoal global newGoalPose global armTorqueActive global attemps ###Communication with dynamixels: global dynMan1 print "JustinaHardwareRightArm.->Trying to open port " + portName1 + " at " + str( portBaud1) dynMan1 = Dynamixel.DynamixelMan(portName1, portBaud1) msgCurrentPose = Float32MultiArray() msgCurrentGripper = Float32() msgBatery = Float32() msgObjOnHand = Bool() msgCurrentPose.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] msgCurrentGripper.data = 0.0 msgBatery = 0.0 msgObjOnHand = False curretPos = [0, 0, 0, 0, 0, 0, 0, 0] bitsPerRadian = (4095) / ((360) * (3.141592 / 180)) bitValues = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] lastValues = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] goalPos = [0, 0, 0, 0, 0, 0, 0] speedsGoal = [0, 0, 0, 0, 0, 0, 0] newGoalPose = False objOnHand = False torqueGripper = 0 currentLoad_D21 = 0 currentLoad_D22 = 0 gripperGoal_1 = zero_gripper[0] gripperGoal_2 = zero_gripper[1] attemps = 0 i = 0 ############################## ### Connection with ROS #### rospy.init_node("right_arm") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = [ "ra_1_joint", "ra_2_joint", "ra_3_joint", "ra_4_joint", "ra_5_joint", "ra_6_joint", "ra_7_joint", "ra_grip_left", "ra_grip_right" ] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] subPos = rospy.Subscriber("/hardware/right_arm/goal_pose", Float32MultiArray, callbackArmPos) subGripper = rospy.Subscriber("/hardware/right_arm/goal_gripper", Float32, callbackGripperPos) subTorqueGripper = rospy.Subscriber("/hardware/right_arm/torque_gripper", Float32, callbackGripperTorque) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubArmPose = rospy.Publisher("right_arm/current_pose", Float32MultiArray, queue_size=1) pubGripper = rospy.Publisher("right_arm/current_gripper", Float32, queue_size=1) pubObjOnHand = rospy.Publisher("right_arm/object_on_hand", Bool, queue_size=1) pubBatery = rospy.Publisher("/hardware/robot_state/right_arm_battery", Float32, queue_size=1) ##################### ## Dynamixel ## for i in range(9): #dynMan1.SetDGain(i, 25) #dynMan1.SetPGain(i, 16) #dynMan1.SetIGain(i, 6) dynMan1.SetDGain(i, 0) dynMan1.SetPGain(i, 32) dynMan1.SetIGain(i, 0) ### Set servos features for i in range(9): dynMan1.SetMaxTorque(i, 1023) dynMan1.SetTorqueLimit(i, 768) dynMan1.SetHighestLimitTemperature(i, 80) dynMan1.SetAlarmShutdown(i, 0b00000100) #Dynamixel gripper on position Mode... dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 4095) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 4095) dynMan1.SetMovingSpeed(7, speedGripper) dynMan1.SetMovingSpeed(8, speedGripper) # Set torque_active for each servo for i in range(7): dynMan1.SetTorqueEnable(i, 1) dynMan1.SetTorqueEnable(7, 1) dynMan1.SetTorqueEnable(8, 1) # Set initial pos for each servo #for i in range(6): # dynMan1.SetGoalPosition(i, zero_arm[i]) #dynMan1.SetGoalPosition(6, 2900) #dynMan1.SetGoalPosition(7, zero_gripper[0]) #dynMan1.SetGoalPosition(8, zero_gripper[1]) loop = rospy.Rate(30) while not rospy.is_shutdown(): #### Refresh arm_position #### if newGoalPose: newGoalPose = False for i in range(7): dynMan1.SetTorqueLimit(i, 768) dynMan1.SetTorqueEnable(i, True) dynMan1.SetMovingSpeed(i, speedsGoal[i]) dynMan1.SetGoalPosition(i, goalPos[i]) rospy.sleep(0.05) #### Refresh gripper_pos #### if attemps < 50: if gripperTorqueActive: dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 0) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 0) dynMan1.SetTorqueLimit(7, torqueGripper) dynMan1.SetTorqueLimit(8, torqueGripper) dynMan1.SetTorqueVale(7, speedGripper, torqueGripperCCW1) dynMan1.SetTorqueVale(8, speedGripper, torqueGripperCCW2) currentLoad_D21 = dynMan1.GetPresentLoad(7) currentLoad_D22 = dynMan1.GetPresentLoad(8) else: dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 4095) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 4095) dynMan1.SetTorqueLimit(7, 500) dynMan1.SetTorqueLimit(8, 500) dynMan1.SetMovingSpeed(7, 200) dynMan1.SetMovingSpeed(8, 200) dynMan1.SetGoalPosition(7, gripperGoal_1) dynMan1.SetGoalPosition(8, gripperGoal_2) objOnHand = False ## This counter is reseated in the callback attemps += 1 #### Refresh arms_position's readings ##### for i in range(9): bitValues[i] = dynMan1.GetPresentPosition(i) if (bitValues[i] == 0): bitValues[i] = lastValues[i] else: lastValues[i] = bitValues[i] # CurrentLoad > 1023 means the oposite direction of load if currentLoad_D21 > 1023: currentLoad_D21 -= 1023 if currentLoad_D22 > 1023: currentLoad_D22 -= 1023 if gripperTorqueActive: currentLoad_D21 = (currentLoad_D21 + currentLoad_D22) / 2 if currentLoad_D21 > 200 and posD21 > -0.2: objOnHand = True else: objOnHand = False pos0 = float((zero_arm[0] - bitValues[0]) / bitsPerRadian) pos1 = float(-(zero_arm[1] - bitValues[1]) / bitsPerRadian) pos2 = float(-(zero_arm[2] - bitValues[2]) / bitsPerRadian) pos3 = float(-(zero_arm[3] - bitValues[3]) / bitsPerRadian) pos4 = float(-(zero_arm[4] - bitValues[4]) / bitsPerRadian) pos5 = float((zero_arm[5] - bitValues[5]) / bitsPerRadian) pos6 = float(-(zero_arm[6] - bitValues[6]) / bitsPerRadian) posD21 = float((zero_gripper[0] - bitValues[7]) / bitsPerRadian) posD22 = float(-(zero_gripper[1] - bitValues[8]) / bitsPerRadian) jointStates.header.stamp = rospy.Time.now() jointStates.position[0] = pos0 jointStates.position[1] = pos1 jointStates.position[2] = pos2 jointStates.position[3] = pos3 jointStates.position[4] = pos4 jointStates.position[5] = pos5 jointStates.position[6] = pos6 jointStates.position[7] = posD21 jointStates.position[8] = posD22 msgCurrentPose.data[0] = pos0 msgCurrentPose.data[1] = pos1 msgCurrentPose.data[2] = pos2 msgCurrentPose.data[3] = pos3 msgCurrentPose.data[4] = pos4 msgCurrentPose.data[5] = pos5 msgCurrentPose.data[6] = pos6 msgCurrentGripper.data = posD21 msgObjOnHand = objOnHand pubJointStates.publish(jointStates) pubArmPose.publish(msgCurrentPose) pubGripper.publish(msgCurrentGripper) pubObjOnHand.publish(msgObjOnHand) if i == 20: msgBatery = float(dynMan1.GetPresentVoltage(2) / 10.0) pubBatery.publish(msgBatery) i = 0 i += 1 loop.sleep() dynMan1.SetTorqueDisable(0) dynMan1.SetTorqueDisable(1) dynMan1.SetTorqueDisable(2) dynMan1.SetTorqueDisable(3) dynMan1.SetTorqueDisable(4) dynMan1.SetTorqueDisable(5) dynMan1.SetTorqueDisable(6) dynMan1.Close()