コード例 #1
0
robot_joints = [
    'ExoLeftHip', 'ExoLeftKnee', 'ExoLeftAnkle', 'ExoRightHip', 'ExoRightKnee',
    'ExoRightAnkle', 'ExoHipCrutches'
]
LARRY = Human.Human(_client, "human", body_joints, 0, 0)
LARRE = Exoskeleton.Exoskeleton(_client, "exo", robot_joints, 56, 1.56)
# LARRE.handle.set_rpy(0.25, 0, 0)
# LARRE.handle.set_pos(0, 0, 1.0)
Dyn = DynController.DynController(LARRE, Kp, Kd)

#mpc = MPController.MPController(LARRE, LARRE.get_runner())

# lqr = LQRController.LQRController(LARRE, LARRE.get_runner())
# controllers = {'Dyn': Dyn,
#                "LQR":lqr}

# lqr = LQRController.LQRController(LARRE, LARRE.get_runner())
controllers = {'Dyn': Dyn}

cnrl = ControllerNode.ControllerNode(LARRE, controllers)
#

# while True:
#     fk = LARRE.fk()
#     print(fk["right_hip"])
# while True:
#     LARRE.calculate_torque()
LARRE.handle.set_rpy(0, 0, 0)
LARRE.handle.set_pos(0.0, 0, 1.0)
machine = StateMachine.ExoStateMachine(LARRE)
コード例 #2
0
#!/usr/bin/env python
import sys
from os import sys, path
sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))

from ambf_walker.msg import DesiredJoints
from StateMachines import StateMachine
from Controller import ControllerNode
from Model import Exoskeleton
import rospy
from ambf_client import Client

_client = Client()
_client.connect()
rate = rospy.Rate(1000)
LARRE = Exoskeleton.Exoskeleton(_client, 56, 1.56)
cnrl = ControllerNode.ControllerNode(LARRE)
machine = StateMachine.ExoStateMachineTest(LARRE)