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()