def __init__(self):
self.server = actionlib.SimpleActionServer(
"SetJointPosition", SetJointPositionAction, self.execute, False)
self.server.start()
self.config = ServoConfig("ernest.ini")
self.network = ServoNetwork(self.config)
self.rshoulder = self.network.get_servo_by_label("rshoulder")
self.rshoulder.synchronized = False
self.rshoulder.max_torque = 1023
self.rshoulder.torque_limit = 1023
self.rshoulder.moving_speed = 0
self.rshoulder.goal_position = RSHOULDER_FRONT
self.rshoulder.torque_enable = 1
self.lshoulder = self.network.get_servo_by_label("lshoulder")
self.lshoulder.synchronized = False
self.lshoulder.max_torque = 1023
self.lshoulder.torque_limit = 1023
self.lshoulder.moving_speed = 0
self.lshoulder.goal_position = LSHOULDER_FRONT
self.lshoulder.torque_enable = 1
self.head = self.network.get_servo_by_label("head")
self.head.synchronized = False
self.head.max_torque = 1023
self.head.torque_limit = 1023
self.head.moving_speed = 0
self.head.goal_position = HEAD_FORWARD
self.head.torque_enable = 1
self.head_load = rospy.Publisher(
"/ernest_body/motors/head", MotorState, queue_size=1)
self.lock = threading.RLock()
class DynamixelJointServer:
def __init__(self):
self.server = actionlib.SimpleActionServer(
"SetJointPosition", SetJointPositionAction, self.execute, False)
self.server.start()
self.config = ServoConfig("ernest.ini")
self.network = ServoNetwork(self.config)
self.rshoulder = self.network.get_servo_by_label("rshoulder")
self.rshoulder.synchronized = False
self.rshoulder.max_torque = 1023
self.rshoulder.torque_limit = 1023
self.rshoulder.moving_speed = 0
self.rshoulder.goal_position = RSHOULDER_FRONT
self.rshoulder.torque_enable = 1
self.lshoulder = self.network.get_servo_by_label("lshoulder")
self.lshoulder.synchronized = False
self.lshoulder.max_torque = 1023
self.lshoulder.torque_limit = 1023
self.lshoulder.moving_speed = 0
self.lshoulder.goal_position = LSHOULDER_FRONT
self.lshoulder.torque_enable = 1
self.head = self.network.get_servo_by_label("head")
self.head.synchronized = False
self.head.max_torque = 1023
self.head.torque_limit = 1023
self.head.moving_speed = 0
self.head.goal_position = HEAD_FORWARD
self.head.torque_enable = 1
self.head_load = rospy.Publisher(
"/ernest_body/motors/head", MotorState, queue_size=1)
self.lock = threading.RLock()
def execute(self, goal):
self.do()
self.server.set_succeeded()
def do(self, goal):
print goal
print goal.joints
self.lock.acquire()
self.head.goal_position = HEAD_FORWARD + HEAD_MAX
self.lock.release()
import time
time.sleep(1)
self.lock.acquire()
self.head.goal_position = HEAD_FORWARD - HEAD_MAX
self.lock.release()
def set_head_angle(self, req):
print "set_head_angle : %s" % req.angle
angle = ax_clamp_angle(req.angle)
self.lock.acquire()
self.head.goal_position = ax_clamp_position(
HEAD_FORWARD + 1024/300*angle)
self.lock.release()
return 0
def set_rshoulder_angle(self, req):
print "set_rshoulder_angle : %s" % req.angle
angle = ax_clamp_angle(req.angle)
self.lock.acquire()
self.rshoulder.goal_position = ax_clamp_position(
RSHOULDER_FRONT + 1024/300*angle)
self.lock.release()
return 0
def set_lshoulder_angle(self, req):
print "set_lshoulder_angle : %s" % req.angle
angle = ax_clamp_angle(req.angle)
self.lock.acquire()
self.lshoulder.goal_position = ax_clamp_position(
LSHOULDER_FRONT + 1024/300*angle)
self.lock.release()
return 0
def loop(self):
while not rospy.is_shutdown():
ms = MotorState()
self.lock.acquire()
ms.load = self.head.current_load
self.lock.release()
self.head_load.publish(ms)
rospy.Rate(50)
display.set_mode((1,1))
joystick.init()
if joystick.get_count() < 1:
print "please plug a joystick"
sys.exit(1)
j = joystick.Joystick(0)
j.init()
print "ok"
from servo.config import ServoConfig
from servo.network import ServoNetwork
config = ServoConfig()
network = ServoNetwork(config)
DEG90=307
DEG45=int(DEG90/2)
P180=512
P90=P180-DEG90
P270=P180+DEG90
HEAD_FORWARD=int(P180-DEG90/2)
HEAD_MAX=HEAD_FORWARD-10
head = network.get_servo_by_label("head")
head.synchronized = False
head.max_torque = 1023
head.torque_limit = 1023
head.moving_speed = 0
head.goal_position = HEAD_FORWARD
display.set_mode((1,1))
joystick.init()
if joystick.get_count() < 1:
print "please plug a joystick"
sys.exit(1)
j = joystick.Joystick(0)
j.init()
print "ok"
from servo.config import ServoConfig
from servo.network import ServoNetwork
config = ServoConfig()
network = ServoNetwork(config)
arm = network.get_servo_by_label("arm")
arm.synchronized = False
arm.max_torque = 1023
arm.torque_limit = 1023
arm.moving_speed = 0
arm.goal_position = 512
arm.torque_enable = 1
arm_goal_position = 512
hand = network.get_servo_by_label("hand")
hand.synchronized = False
hand.max_torque = 1023
hand.torque_limit = 1023
hand.moving_speed = 0
#!/usr/bin/python
from servo.config import ServoConfig
from servo.network import ServoNetwork
config = ServoConfig()
network = ServoNetwork(config)
head = network.get_servo_by_label("head")
neck = network.get_servo_by_label("neck")
head.goal_position = 400
neck.goal_position = 400
#!/usr/bin/python
from servo.config import ServoConfig
from servo.network import ServoNetwork
from servo.motion import Position
config = ServoConfig()
network = ServoNetwork(config)
network.apply_all(torque_enable=1, moving_speed=1023)
walk_0 = Position("pos/puppy/walk_0")
walk_1 = Position("pos/puppy/walk_1")
walk_2 = Position("pos/puppy/walk_2")
walk_3 = Position("pos/puppy/walk_3")
import time
while True:
walk_0(network)
time.sleep(0.5)
walk_1(network)
time.sleep(0.5)
walk_2(network)
time.sleep(0.5)
walk_3(network)
time.sleep(0.5)
