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)