コード例 #1
0
ファイル: body.py プロジェクト: simkim/ernest
    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()
コード例 #2
0
ファイル: body.py プロジェクト: simkim/ernest
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)
コード例 #3
0
ファイル: example-joystick-robot.py プロジェクト: simkim/ax
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
コード例 #4
0
ファイル: example-joystick.py プロジェクト: simkim/ax
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
コード例 #5
0
ファイル: example.py プロジェクト: simkim/ax
#!/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
コード例 #6
0
ファイル: example-motion.py プロジェクト: simkim/ax
#!/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)