Kd[2, 2] = Kd_ankle
Kp[3, 3] = Kp_hip
Kd[3, 3] = Kd_hip
Kp[4, 4] = Kp_knee
Kd[4, 4] = Kd_knee
Kp[5, 5] = Kp_ankle
Kd[5, 5] = Kd_ankle
_client = Client()
_client.connect()
rate = rospy.Rate(1000)
pend = DoublePendulum.DoublePendulum(_client, "pend",
["head_top", "top_bottom"])
Dyn = DynController.DynController(pend, 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(pend, controllers)
pend.calculate_torque()
# while True:
rate = rospy.Rate(1000)
body_joints = [
'HumanLeftHip', 'HumanLeftKnee', 'HumanLeftAnkle', 'HumanRightHip',
'HumanRightKnee', 'HumanRightAnkle'
]
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()
Kd_FF[2, 2] = Kd_ankle
Kp_FF[3, 3] = Kp_hip
Kd_FF[3, 3] = Kd_hip
Kp_FF[4, 4] = Kp_knee
Kd_FF[4, 4] = Kd_knee
Kp_FF[5, 5] = Kp_ankle
Kd_FF[5, 5] = Kd_ankle
_client = Client()
_client.connect()
rate = rospy.Rate(1000)
robot_joints = ['ExoLeftHip', 'ExoLeftKnee', 'ExoLeftAnkle',
'ExoRightHip', 'ExoRightKnee', 'ExoRightAnkle', 'ExoHipCrutches']
#joints = ['Hip-Leftthigh', 'Leftthigh-Leftshank', 'Leftshank-Leftfoot', 'Hip-Rightthigh', 'Rightthigh-Rightshank', 'Rightshank-Rightfoot', 'Hip-Cylinder']
LARRE = Exoskeleton.Exoskeleton(_client, "exo", robot_joints, 56, 1.56)
Dyn = DynController.DynController(LARRE, Kp_Dyn, Kd_Dyn)
FF = FeedForwardController.FeedForwardController(LARRE, Kp_FF, Kd_FF)
controllers = {'Dyn': Dyn,
"FF": FF}
cnrl = ControllerNode.ControllerNode(LARRE, controllers)
machine = StateMachine.ExoStateMachine(LARRE)