def fk(self):
fk = {}
point_local = np.array([0.0, 0.0, 0.0])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_thigh, point_local)
fk["left_hip"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_shank, point_local)
fk["left_knee"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.left_foot, point_local)
fk["left_ankle"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_thigh, point_local)
fk["right_hip"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_shank, point_local)
fk["right_knee"] = Point.Point(data[0], data[1], data[2])
data = rbdl.CalcBodyToBaseCoordinates(self.rbdl_model, self.q,
self.right_foot, point_local)
fk["right_ankle"] = Point.Point(data[0], data[1], data[2])
q_left = self.get_left_leg().ankle.angle.z
q_right = self.get_right_leg().ankle.angle.z
fk["left_toe"] = Point.Point(0, 0, 0)
fk["left_toe"].x = fk["left_ankle"].x - 0.8 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_toe"].y = fk["left_ankle"].y - 0.8 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_toe"].z = fk["left_ankle"].z - 0.05 + 0.8 * (
8.0 / 100.0) * self._height * np.sin(q_left)
fk["left_heel"] = Point.Point(0, 0, 0)
fk["left_heel"].x = fk["left_ankle"].x + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_heel"].y = fk["left_ankle"].y + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_left)
fk["left_heel"].z = fk["left_ankle"].z - 0.05 + 0.2 * (
8.0 / 100.0) * self._height * np.sin(q_left)
fk["right_toe"] = Point.Point(0, 0, 0)
fk["right_toe"].x = fk["right_ankle"].x - 0.8 * (
8.0 / 100.0) * 1.57 * np.cos(q_right)
fk["right_toe"].y = fk["right_ankle"].y - 0.8 * (
8.0 / 100.0) * 1.57 * np.cos(q_right)
fk["right_toe"].z = fk["right_ankle"].z - 0.05 + 0.8 * (
8.0 / 100.0) * self._height * np.sin(q_right)
fk["right_heel"] = Point.Point(0, 0, 0)
fk["right_heel"].x = fk["right_ankle"].x + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_right)
fk["right_heel"].y = fk["right_ankle"].y + 0.2 * (
8.0 / 100.0) * self._height * np.cos(q_right)
fk["right_heel"].z = fk["right_ankle"].z - 0.05 + 0.2 * (
8.0 / 100.0) * self._height * np.sin(q_right)
return fk
def __init__(self, client, mass, height):
super(Exoskeleton, self).__init__(client)
self.q = 7 * [0.0]
self.qd = 7 * [0.0]
self._handle = self._client.get_obj_handle('Hip')
time.sleep(2)
self._mass = mass
self._height = height
self._model = self.dynamic_model()
left_joints = {}
right_joints = {}
for joint in (left_joints, right_joints):
for output in ["Hip", "Knee", "Ankle"]:
angle = Point.Point(0, 0, 0)
force = Point.Point(0, 0, 0)
moment = Point.Point(0, 0, 0)
power = Point.Point(0, 0, 0)
joint[output] = Joint.Joint(angle, moment, power, force)
self._left_leg = Leg.Leg(left_joints["Hip"], left_joints["Knee"],
left_joints["Ankle"])
self._right_leg = Leg.Leg(right_joints["Hip"], right_joints["Knee"],
right_joints["Ankle"])
self._state = (self._q, self._qd)
self.subs = []
topics = [
"right_thigh", "right_shank", "right_foot", "left_thigh",
"left_shank", "left_foot"
]
for name in topics:
self.subs.append(
rospy.Subscriber(name,
RigidBodyState,
self.force_cb,
callback_args=name))
self._updater.start()
def leg_sensor_callback(self, flt, blt, fls, bls, frt, brt, frs, brs):
force_flt = Point.Point(flt.wrench.force.x, flt.wrench.force.y, flt.wrench.force.z)
force_blt = Point.Point(blt.wrench.force.x, blt.wrench.force.y, blt.wrench.force.z)
force_fls = Point.Point(fls.wrench.force.x, fls.wrench.force.y, fls.wrench.force.z)
force_bls = Point.Point(bls.wrench.force.x, bls.wrench.force.y, bls.wrench.force.z)
force_frt = Point.Point(frt.wrench.force.x, frt.wrench.force.y, frt.wrench.force.z)
force_brt = Point.Point(brt.wrench.force.x, brt.wrench.force.y, brt.wrench.force.z)
force_frs = Point.Point(frs.wrench.force.x, frs.wrench.force.y, frs.wrench.force.z)
force_brs = Point.Point(brs.wrench.force.x, brs.wrench.force.y, brs.wrench.force.z)
self._left_leg.hip.force = force_flt
self._left_leg.knee.force = force_fls
self._right_leg.hip.force = force_frt
self._right_leg.knee.force = force_frs
def __init__(self, client, model_name, joints, model_path, use_gravity=False):
super(ExoskeletonServer, self).__init__(client, model_name=model_name, joint_names=joints, model_path=model_path)
self._handle = self._client.get_obj_handle('ExoHip')
self._use_gravity = use_gravity
# Update to current
self.prox = {}
self.prox["LeftSideProx"] = rospy.Publisher('left_leg', PointCloud, queue_size=10)
self.prox["RightSideProx"] = rospy.Publisher('right_leg', PointCloud, queue_size=10)
self.make_dynamic_model("exo", model_path)
time.sleep(4)
self._state = (self._q, self._qd)
rospy.Subscriber("/ambf/env/LeftSideProx/State", SensorState, self.prox_callback)
rospy.Subscriber("/ambf/env/RightSideProx/State", SensorState, self.prox_callback)
rospy.Subscriber("/ambf/env/LeftFootProx/State", SensorState, self.left_foot_prox_callback)
rospy.Subscriber("/ambf/env/RightFootProx/State", SensorState, self.right_foot_prox_callback)
self._left_thigh_sensorF_sub = message_filters.Subscriber("/ambf/env/FrontSensorLeftThigh/State", RigidBodyState)
self._left_thigh_sensorB_sub = message_filters.Subscriber("/ambf/env/BackSensorLeftThigh/State", RigidBodyState)
self._left_shank_sensorF_sub = message_filters.Subscriber("/ambf/env/FrontSensorLeftShank/State", RigidBodyState)
self._left_shank_sensorB_sub = message_filters.Subscriber("/ambf/env/BackSensorLeftShank/State", RigidBodyState)
self._right_thigh_sensorF_sub = message_filters.Subscriber("/ambf/env/FrontSensorRightThigh/State", RigidBodyState)
self._right_thigh_sensorB_sub = message_filters.Subscriber("/ambf/env/BackSensorRightThigh/State", RigidBodyState)
self._right_shank_sensorF_sub = message_filters.Subscriber("/ambf/env/FrontSensorRightShank/State", RigidBodyState)
self._right_shank_sensorB_sub = message_filters.Subscriber("/ambf/env/BackSensorRightShank/State", RigidBodyState)
self._leg_sensor_ls = [self._left_thigh_sensorF_sub, self._left_thigh_sensorB_sub,
self._left_shank_sensorF_sub, self._left_shank_sensorB_sub,
self._right_thigh_sensorF_sub, self._right_thigh_sensorB_sub,
self._right_shank_sensorF_sub, self._right_shank_sensorB_sub]
self._leg_sensor_cb = message_filters.TimeSynchronizer(self._leg_sensor_ls, 1)
self._leg_sensor_cb.registerCallback(self.leg_sensor_callback)
self._left_foot_force_sensor = []
self._right_foot_force_sensor = []
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_sensor1_sub = message_filters.Subscriber("/ambf/env/SensorLeftFoot1Tab/State", RigidBodyState)
self._left_foot_sensor2_sub = message_filters.Subscriber("/ambf/env/SensorLeftFoot2Tab/State", RigidBodyState)
self._left_foot_sensor3_sub = message_filters.Subscriber("/ambf/env/SensorLeftFoot3Tab/State", RigidBodyState)
self._right_foot_sensor1_sub = message_filters.Subscriber("/ambf/env/SensorRightFoot1Tab/State", RigidBodyState)
self._right_foot_sensor2_sub = message_filters.Subscriber("/ambf/env/SensorRightFoot2Tab/State", RigidBodyState)
self._right_foot_sensor3_sub = message_filters.Subscriber("/ambf/env/SensorRightFoot3Tab/State", RigidBodyState)
self._foot_sensor_ls = [self._left_foot_sensor1_sub, self._left_foot_sensor2_sub, self._left_foot_sensor3_sub,
self._right_foot_sensor1_sub, self._right_foot_sensor2_sub, self._right_foot_sensor3_sub]
self._foot_sensor_cb = message_filters.TimeSynchronizer(self._foot_sensor_ls, 1)
self._foot_sensor_cb.registerCallback(self.foot_sensor_callback)
self._right_foot_prox = SensorState()
self._left_foot_prox = SensorState()
self._updater.start()
def foot_sensor_callback(self, lf1, lf2, lf3, rf1, rf2, rf3):
force_lf1 = Point.Point(lf1.wrench.force.x, lf1.wrench.force.y, lf1.wrench.force.z)
force_lf2 = Point.Point(lf2.wrench.force.x, lf2.wrench.force.y, lf2.wrench.force.z)
force_lf3 = Point.Point(lf3.wrench.force.x, lf3.wrench.force.y, lf3.wrench.force.z)
force_rf1 = Point.Point(rf1.wrench.force.x, rf1.wrench.force.y, rf1.wrench.force.z)
force_rf2 = Point.Point(rf2.wrench.force.x, rf2.wrench.force.y, rf2.wrench.force.z)
force_rf3 = Point.Point(rf3.wrench.force.x, rf3.wrench.force.y, rf3.wrench.force.z)
self.left_foot_force_sensor = [force_lf1, force_lf2, force_lf3]
self.right_foot_force_sensor = [force_rf1, force_rf2, force_rf3]
def force_cb(self, msg, name):
if "right" in name:
leg = self._right_leg
else:
leg = self._left_leg
point = Point.Point(msg.wrench.force.x, msg.wrench.force.y,
msg.wrench.force.z)
if "thigh" in name:
leg.hip.force = point
elif "shank" in name:
leg.knee.force = point
elif "foot" in name:
leg.ankle.force = point
def __initlize(self):
self._handle = self._client.get_obj_handle('ExoHip')
# Update to current
self.prox = {}
self.prox["LeftSideProx"] = rospy.Publisher('left_leg',
PointCloud,
queue_size=10)
self.prox["RightSideProx"] = rospy.Publisher('right_leg',
PointCloud,
queue_size=10)
time.sleep(4)
self.rbdl_model = self.make_dynamic_model()
left_joints = {}
right_joints = {}
for joint in (left_joints, right_joints):
for output in ["Hip", "Knee", "Ankle"]:
angle = Point.Point(0, 0, 0)
force = Point.Point(0, 0, 0)
moment = Point.Point(0, 0, 0)
power = Point.Point(0, 0, 0)
joint[output] = Joint.Joint(angle, moment, power, force)
self._left_leg = Leg.Leg(left_joints["Hip"], left_joints["Knee"],
left_joints["Ankle"])
self._right_leg = Leg.Leg(right_joints["Hip"], right_joints["Knee"],
right_joints["Ankle"])
self._state = (self._q, self._qd)
# START ATTEMPT
# self._left_thigh_sensorF = Point.Point(0, 0, 0)
# self._left_thigh_sensorB = Point.Point(0, 0, 0)
# self._left_shank_sensorF = Point.Point(0, 0, 0)
# self._left_shank_sensorB = Point.Point(0, 0, 0)
# self._right_thigh_sensorF = Point.Point(0, 0, 0)
# self._right_thigh_sensorB = Point.Point(0, 0, 0)
# self._right_shank_sensorF = Point.Point(0, 0, 0)
# self._right_shank_sensorB = Point.Point(0, 0, 0)
rospy.Subscriber("/ambf/env/LeftSideProx/State", SensorState,
self.prox_callback)
rospy.Subscriber("/ambf/env/RightSideProx/State", SensorState,
self.prox_callback)
rospy.Subscriber("/ambf/env/LeftFootProx/State", SensorState,
self.left_foot_prox_callback)
rospy.Subscriber("/ambf/env/RightFootProx/State", SensorState,
self.right_foot_prox_callback)
self._left_thigh_sensorF_sub = message_filters.Subscriber(
"/ambf/env/FrontSensorLeftThigh/State", RigidBodyState)
self._left_thigh_sensorB_sub = message_filters.Subscriber(
"/ambf/env/BackSensorLeftThigh/State", RigidBodyState)
self._left_shank_sensorF_sub = message_filters.Subscriber(
"/ambf/env/FrontSensorLeftShank/State", RigidBodyState)
self._left_shank_sensorB_sub = message_filters.Subscriber(
"/ambf/env/BackSensorLeftShank/State", RigidBodyState)
self._right_thigh_sensorF_sub = message_filters.Subscriber(
"/ambf/env/FrontSensorRightThigh/State", RigidBodyState)
self._right_thigh_sensorB_sub = message_filters.Subscriber(
"/ambf/env/BackSensorRightThigh/State", RigidBodyState)
self._right_shank_sensorF_sub = message_filters.Subscriber(
"/ambf/env/FrontSensorRightShank/State", RigidBodyState)
self._right_shank_sensorB_sub = message_filters.Subscriber(
"/ambf/env/BackSensorRightShank/State", RigidBodyState)
self._leg_sensor_ls = [
self._left_thigh_sensorF_sub, self._left_thigh_sensorB_sub,
self._left_shank_sensorF_sub, self._left_shank_sensorB_sub,
self._right_thigh_sensorF_sub, self._right_thigh_sensorB_sub,
self._right_shank_sensorF_sub, self._right_shank_sensorB_sub
]
self._leg_sensor_cb = message_filters.TimeSynchronizer(
self._leg_sensor_ls, 1)
self._leg_sensor_cb.registerCallback(self.leg_sensor_callback)
self._left_foot_force_sensor = []
self._right_foot_force_sensor = []
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._right_foot_force_sensor.append(Point.Point(0, 0, 0))
self._left_foot_sensor1_sub = message_filters.Subscriber(
"/ambf/env/SensorLeftFoot1Tab/State", RigidBodyState)
self._left_foot_sensor2_sub = message_filters.Subscriber(
"/ambf/env/SensorLeftFoot2Tab/State", RigidBodyState)
self._left_foot_sensor3_sub = message_filters.Subscriber(
"/ambf/env/SensorLeftFoot3Tab/State", RigidBodyState)
self._right_foot_sensor1_sub = message_filters.Subscriber(
"/ambf/env/SensorRightFoot1Tab/State", RigidBodyState)
self._right_foot_sensor2_sub = message_filters.Subscriber(
"/ambf/env/SensorRightFoot2Tab/State", RigidBodyState)
self._right_foot_sensor3_sub = message_filters.Subscriber(
"/ambf/env/SensorRightFoot3Tab/State", RigidBodyState)
self._foot_sensor_ls = [
self._left_foot_sensor1_sub, self._left_foot_sensor2_sub,
self._left_foot_sensor3_sub, self._right_foot_sensor1_sub,
self._right_foot_sensor2_sub, self._right_foot_sensor3_sub
]
self._foot_sensor_cb = message_filters.TimeSynchronizer(
self._foot_sensor_ls, 1)
self._foot_sensor_cb.registerCallback(self.foot_sensor_callback)
self._right_foot_prox = SensorState()
self._left_foot_prox = SensorState()
self._updater.start()
def get(self, ind):
return Point.Point(self.x[ind], self.y[ind], self.z[ind])
def __getitem__(self, item):
if isinstance(item, slice):
return PointArray(self.x[item], self.y[item], self.z[item])
return Point.Point(self.x[item], self.y[item], self.z[item])
import numpy as np
from GaitAnaylsisToolkit.LearningTools.Trainer import GMMTrainer
from GaitAnaylsisToolkit.Session import ViconGaitingTrial
from GaitAnaylsisToolkit.LearningTools.Runner import GMMRunner
from GaitCore.Core import Point
import matplotlib.pyplot as plt
from dtw import dtw
import numpy.polynomial.polynomial as poly
frames = {}
frames["stairA"] = [Point.Point(0, 0, 0),
Point.Point(63, 0, 0),
Point.Point(0, 42, 0),
Point.Point(63, 49, 0)]
frames["stairB"] = [Point.Point(0, 0, 0),
Point.Point(49, 0, 0),
Point.Point(28, 56, 0),
Point.Point(70, 70, 0)]
file13 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_02/subject_02_stair_config1_00.csv"
file12 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_01/subject_01 stairconfig1_02.csv"
file11 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_00/subject_00 stairconfig2_00.csv"
file10 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_10/subject_10 stairclimbing_config1_01.csv"
file09 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_09/subject_09 stairclimbing_config1_00.csv"
file07 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_07/subject_07 stairclimbing_config1_00.csv"
file06 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_06/subject_06 stairclimbing_config1_02.csv"
file05 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_05/subject_05_stair_config1_01.csv"
file03 = "/home/nathanielgoldfarb/AIM_GaitData/Gaiting_stairs/subject_03/subject_03_stair_config0_02.csv"
import sys
import matplotlib.pyplot as plt
from scipy import signal
import numpy as np
from GaitAnaylsisToolkit.LearningTools.Trainer import TPGMMTrainer, GMMTrainer
from GaitAnaylsisToolkit.LearningTools.Runner import GMMRunner, TPGMMRunner
from GaitAnaylsisToolkit.Session import ViconGaitingTrial
from Vicon import Vicon
from GaitCore.Core import Point
frames = {}
frames["stairA"] = [
Point.Point(0, 0, 0),
Point.Point(63, 0, 0),
Point.Point(0, 42, 0),
Point.Point(63, 49, 0)
]
frames["stairB"] = [
Point.Point(0, 0, 0),
Point.Point(49, 0, 0),
Point.Point(28, 56, 0),
Point.Point(70, 70, 0)
]
file13 = "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_02/subject_02_stair_config1_00.csv"
file12 = "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_01/subject_01 stairconfig1_02.csv"
file11 = "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_00/subject_00 stairconfig2_00.csv"
file10 = "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_10/subject_10 stairclimbing_config1_01.csv"
file09 = "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_09/subject_09 stairclimbing_config1_00.csv"
return self
def __next__(self):
if self.n < self.__len__():
point = self.get(self.n)
self.n += 1
return point
else:
raise StopIteration
def __len__(self):
return len(self._x)
def __abs__(self):
x = []
y = []
z = []
for i in range(len(self.x)):
x.append(abs(self.x[i]))
y.append(abs(self.y[i]))
z.append(abs(self.z[i]))
return PointArray(x, y, z)
if __name__ == '__main__':
p = PointArray.init_point_array()
p.append(Point.Point(5, 5, 5))
p.append(Point.Point(5, 5, 5))
print(5 * p)
