711 lines
27 KiB
Python
711 lines
27 KiB
Python
from typing import List, Callable
|
|
import numpy as np
|
|
|
|
|
|
from typing import TYPE_CHECKING
|
|
if TYPE_CHECKING:
|
|
from Physics_Elements import StateSpace, Joint, Spring, SharedRigidActuator, SharedSpring
|
|
|
|
ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
|
|
|
|
|
|
'''
|
|
TODO
|
|
1. Change spring_force to just force
|
|
Include getters and setters, lest important for attributes
|
|
2. Add Object_Sharing objects to this file
|
|
'''
|
|
|
|
|
|
def stiffness_function_const_then_rigid(length, unstretched_length):
|
|
percent_length = 0.9 # For the first bit of length in compression, it follors k*x
|
|
k = 1000
|
|
|
|
if length < (1-percent_length)* unstretched_length:
|
|
# It is really compressed, so make k = k *10
|
|
k = k + k*10/(1 + np.exp(-10*(unstretched_length - length - percent_length*unstretched_length)))
|
|
return k
|
|
|
|
def damping_force_x(mass: 'Joint', t):
|
|
x_vel = mass.statespace.get_vel()[0]
|
|
a = 1
|
|
b = 0.01
|
|
return x_vel*a*-1 + x_vel**2 * b*-1
|
|
|
|
def damping_force_y(mass: 'Joint', t):
|
|
y_vel = mass.statespace.get_vel()[1]
|
|
a = 1
|
|
b = 0.01
|
|
return y_vel*a*-1 + y_vel**2 * b*-1
|
|
|
|
def damping_force_z(mass: 'Joint', t):
|
|
z_vel = mass.statespace.get_vel()[2]
|
|
a = 1
|
|
b = 0.01
|
|
return z_vel*a*-1 + z_vel**2 * b*-1
|
|
|
|
'''
|
|
TODO
|
|
'''
|
|
class StateSpace():
|
|
def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR), integration_method="simple"):
|
|
self.pos = pos
|
|
self.vel = vel
|
|
self.accel = accel
|
|
|
|
self.force = force
|
|
|
|
self.integration_method = self.parse_integration_method(integration_method)
|
|
|
|
self.max_saved = 2
|
|
self.previous_positions: list[list[float, float, float]] = []
|
|
self.previous_vel: list[list[float, float, float]] = []
|
|
self.previous_accel: list[list[float, float, float]] = []
|
|
|
|
self.verlet_constants = [
|
|
[],
|
|
[],
|
|
[-1, 2, 1],
|
|
[0, -1, 2, 1],
|
|
[1/11, -4/11, -6/11, 20/11, 12/11],
|
|
]
|
|
|
|
def parse_integration_method(self, method_str):
|
|
if method_str == "simple":
|
|
return 1
|
|
elif method_str == "verlet":
|
|
return 2
|
|
elif method_str == "adams-bashforth":
|
|
return 3
|
|
|
|
|
|
def integrate(self, delta_t):
|
|
'''
|
|
NOTE: This is run after accel has been updated.
|
|
'''
|
|
if self.integration_method == 1:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
self.pos = self.pos + self.vel * delta_t
|
|
elif self.integration_method == 2:
|
|
# Verlet integration
|
|
self.save_last()
|
|
if delta_t >= 0.005:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
self.pos = self.pos + self.vel * delta_t
|
|
else:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
self.verlet_integration(delta_t)
|
|
elif self.integration_method == 3: # Adams-Bashforth
|
|
self.save_last()
|
|
if delta_t >= 0.005:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
self.pos = self.pos + self.vel * delta_t
|
|
else:
|
|
self.adams_bashforth_integration(delta_t)
|
|
|
|
def print(self):
|
|
print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
|
|
|
|
def adams_bashforth_integration(self, delta_t):
|
|
'''
|
|
Testing.
|
|
2 points seems to damp the system. 3 does better at maintaining. Above 3 it is worse
|
|
'''
|
|
num_prev_accel = len(self.previous_accel)
|
|
match num_prev_accel:
|
|
case 0 | 1:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
case 2:
|
|
self.vel = self.vel + delta_t * (1.5 * self.previous_accel[-1] - 0.5 * self.previous_accel[-2])
|
|
case 3:
|
|
self.vel = self.vel + delta_t * (23/12 * self.previous_accel[-1] - 4/3 * self.previous_accel[-2] + 5/12 * self.previous_accel[-3])
|
|
case 4:
|
|
self.vel = self.vel + delta_t * (55/24 * self.previous_accel[-1] - 59/24 * self.previous_accel[-2] + 37/24 * self.previous_accel[-3] - 9/24 * self.previous_accel[-4])
|
|
case 5:
|
|
self.vel = self.vel + delta_t * ((1901 * self.previous_accel[-1] - 2774 * self.previous_accel[-2] + 2616 * self.previous_accel[-3] - 1274 * self.previous_accel[-4] + 251 * self.previous_accel[-5]) / 720)
|
|
case _:
|
|
self.vel = self.vel + self.accel * delta_t
|
|
self.pos = self.pos + self.vel * delta_t
|
|
|
|
def verlet_integration(self, delta_t):
|
|
num_prev_positions = len(self.previous_positions)
|
|
match num_prev_positions:
|
|
case 0 | 1:
|
|
self.pos = self.pos + self.vel * delta_t
|
|
case 2:
|
|
new_pos = np.array([0.0, 0.0, 0.0])
|
|
for i in range(2):
|
|
new_pos += self.verlet_constants[2][i] * self.previous_positions[i]
|
|
new_pos += self.verlet_constants[2][-1] * delta_t * self.accel * delta_t
|
|
self.pos = new_pos
|
|
case 3:
|
|
new_pos = np.array([0.0, 0.0, 0.0])
|
|
for i in range(3):
|
|
new_pos += self.verlet_constants[3][i] * self.previous_positions[i]
|
|
new_pos += self.verlet_constants[3][-1] * delta_t * self.accel * delta_t
|
|
self.pos = new_pos
|
|
case 4:
|
|
new_pos = np.array([0.0, 0.0, 0.0])
|
|
for i in range(4):
|
|
new_pos += self.verlet_constants[4][i] * self.previous_positions[i]
|
|
new_pos += self.verlet_constants[4][-1] * delta_t * self.accel * delta_t
|
|
self.pos = new_pos
|
|
case _:
|
|
self.pos = self.pos + self.vel * delta_t
|
|
|
|
def save_last(self):
|
|
self.previous_positions.append(np.copy(self.pos))
|
|
self.previous_positions = self.previous_positions[-1*self.max_saved:]
|
|
self.previous_vel.append(np.copy(self.vel))
|
|
self.previous_vel = self.previous_vel[-1*self.max_saved:]
|
|
self.previous_accel.append(np.copy(self.accel))
|
|
self.previous_accel = self.previous_accel[-1*self.max_saved:]
|
|
|
|
def save_last_statespace(self):
|
|
if self.last_statespace is None:
|
|
self.last_statespace = StateSpace()
|
|
self.last_statespace.pos = np.copy(self.pos)
|
|
self.last_statespace.vel = np.copy(self.vel)
|
|
self.last_statespace.accel = np.copy(self.accel)
|
|
|
|
def set_pos(self, new_pos):
|
|
self.pos = new_pos
|
|
def set_vel(self, new_vel):
|
|
self.vel = new_vel
|
|
def set_accel(self, new_accel):
|
|
self.accel = new_accel
|
|
def set_force(self, new_force):
|
|
self.force = new_force
|
|
|
|
def get_pos(self):
|
|
return self.pos
|
|
def get_vel(self):
|
|
return self.vel
|
|
def get_accel(self):
|
|
return self.accel
|
|
def get_force(self):
|
|
return self.force
|
|
|
|
class Joint():
|
|
def __init__(self, pos=np.copy(ZERO_VECTOR), mass: float=0.1, fixed: bool=False, name: str="Joint", integration_method:str="simple"):
|
|
self.name = name
|
|
self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR, integration_method=integration_method)
|
|
|
|
self.connected_springs: List[Spring] = []
|
|
self.constant_external_forces: List[List[float, float, float]] = []
|
|
|
|
self.connected_actuators: List[RigidActuator] = []
|
|
|
|
self.variable_external_forces = [[], [], []]
|
|
|
|
self.fixed = fixed
|
|
self.connected_to_rigid_actuator = False
|
|
self.mass = mass
|
|
|
|
self.parent_joints: List[Joint] = []
|
|
self.child_joints: List[Joint] = []
|
|
|
|
self.damping = False
|
|
|
|
self.sharing_attributes = False
|
|
self.shared_attributes = None
|
|
|
|
def is_same(self, other_joint: 'Joint'):
|
|
if self.name == other_joint.get_name():
|
|
return True
|
|
return False
|
|
|
|
def get_statespace(self):
|
|
return self.statespace
|
|
|
|
def get_pos(self):
|
|
return self.statespace.get_pos()
|
|
|
|
def get_vel(self):
|
|
return self.statespace.get_vel()
|
|
|
|
def get_accel(self):
|
|
return self.statespace.get_accel()
|
|
|
|
def set_state_space_pos(self, new_pos):
|
|
self.statespace.set_pos(new_pos)
|
|
|
|
def set_state_space_vel(self, new_vel):
|
|
self.statespace.set_vel(new_vel)
|
|
|
|
def set_connected_to_rigid_actuator(self, state):
|
|
self.connected_to_rigid_actuator = state
|
|
|
|
def get_connected_springs(self):
|
|
return self.connected_springs
|
|
|
|
def get_connected_actuators(self):
|
|
return self.connected_actuators
|
|
|
|
def get_name(self):
|
|
return self.name
|
|
|
|
def add_spring(self, new_spring):
|
|
self.connected_springs.append(new_spring)
|
|
|
|
def add_actuator(self, new_actuator):
|
|
self.connected_actuators.append(new_actuator)
|
|
|
|
def add_constant_force(self, new_force):
|
|
self.constant_external_forces.append(new_force)
|
|
|
|
def integrate_statespace(self, delta_t):
|
|
self.statespace.integrate(delta_t)
|
|
|
|
def add_gravity(self):
|
|
gravity_force = np.array([
|
|
0,
|
|
-9.81 * self.mass,
|
|
0
|
|
])
|
|
self.add_constant_force(gravity_force)
|
|
|
|
def add_variable_force(self, force_vect: list):
|
|
if force_vect[0] is not None:
|
|
self.variable_external_forces[0].append(force_vect[0])
|
|
if force_vect[1] is not None:
|
|
self.variable_external_forces[1].append(force_vect[1])
|
|
if force_vect[2] is not None:
|
|
self.variable_external_forces[2].append(force_vect[2])
|
|
|
|
def calc_net_spring_force(self):
|
|
net_spring_force = 0
|
|
for spring in self.get_connected_springs():
|
|
if self.is_same(spring.get_parent_joint()):
|
|
spring_force = spring.get_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
|
|
else:
|
|
spring_force = -1*spring.get_spring_force()
|
|
net_spring_force += spring_force
|
|
return net_spring_force
|
|
|
|
def calc_net_constant_external_force(self):
|
|
net_external_force = np.copy(ZERO_VECTOR)
|
|
for force in self.constant_external_forces:
|
|
net_external_force += force
|
|
return net_external_force
|
|
|
|
def calc_net_variable_external_force(self, t):
|
|
net_variable_external_force = np.array([0.0, 0.0, 0.0])
|
|
for i in range(3):
|
|
for func in self.variable_external_forces[i]:
|
|
force = func(t)
|
|
net_variable_external_force[i] += force
|
|
return net_variable_external_force
|
|
|
|
def calc_net_force(self, t):
|
|
net_force = np.copy(ZERO_VECTOR)
|
|
if len(self.constant_external_forces) != 0:
|
|
net_force += self.calc_net_constant_external_force()
|
|
if len(self.variable_external_forces) != 0:
|
|
net_force += self.calc_net_variable_external_force(t)
|
|
|
|
net_spring_force = self.calc_net_spring_force()
|
|
|
|
if self.damping == True:
|
|
net_force += self.calc_damping(net_spring_force)
|
|
|
|
net_force += net_spring_force
|
|
|
|
self.statespace.set_force(net_force)
|
|
return self.statespace.get_force()
|
|
|
|
def add_damping(self, vel_ratio=0.25, mom_ratio=None):
|
|
self.damping = True
|
|
if mom_ratio is not None:
|
|
self.damping_vel_ratio = self.mass * mom_ratio
|
|
else:
|
|
self.damping_vel_ratio = vel_ratio
|
|
|
|
def calc_damping(self, spring_force):
|
|
vel_damping = self.damping_vel_ratio * (self.statespace.get_vel()) * -1
|
|
|
|
return vel_damping
|
|
|
|
def calc_accel(self):
|
|
if self.fixed == False and self.connected_to_rigid_actuator == False:
|
|
self.statespace.set_accel(self.statespace.get_force() / self.mass)
|
|
|
|
def is_in_list(self, joints: list['Joint']):
|
|
for j in joints:
|
|
if self.is_same(j):
|
|
return True
|
|
return False
|
|
|
|
def find_parent_joints(self):
|
|
for spring in self.connected_springs:
|
|
if self.is_same(spring.get_child_joint()):
|
|
parent_joint = spring.get_parent_joint()
|
|
if not parent_joint.is_in_list(self.parent_joints):
|
|
self.parent_joints.append(parent_joint)
|
|
|
|
def find_child_joints(self):
|
|
for spring in self.connected_springs:
|
|
if self.is_same(spring.get_parent_joint()):
|
|
child_joint = spring.get_child_joint()
|
|
if not child_joint.is_in_list(self.child_joints):
|
|
self.child_joints.append(child_joint)
|
|
|
|
def print_attributes(self):
|
|
print(f"Joint: {self.name}")
|
|
print(f"Force: {self.statespace.get_force()}")
|
|
print(f"Accel: {self.statespace.get_accel()}")
|
|
print(f"Vel: {self.statespace.get_vel()}")
|
|
print(f"Pos: {self.statespace.get_pos()}")
|
|
|
|
def attach_shared_attributes(self, shared_attributes: 'SharedJoint'):
|
|
if not isinstance(shared_attributes._getvalue(), SharedJoint):
|
|
raise TypeError(f"shared_attributes for Joint {self.name} is not a SharedJoint")
|
|
self.sharing_attributes = True
|
|
self.shared_attributes = shared_attributes
|
|
|
|
self.shared_attributes.set_name(f"{self.name}")
|
|
|
|
self.update_shared_attributes()
|
|
|
|
def update_shared_attributes(self):
|
|
if self.shared_attributes is not None:
|
|
self.shared_attributes.set_statespace(self.statespace)
|
|
|
|
class Spring():
|
|
def __init__(self, parent_joint: 'Joint', child_joint: 'Joint', unstretched_length: float=0, constant_stiffness: float = None, stiffness_func: Callable[[float, float], float]=None, name: str="Spring"):
|
|
self.name = name
|
|
|
|
self.parent_joint = parent_joint
|
|
self.child_joint = child_joint
|
|
|
|
self.unstretched_length = unstretched_length
|
|
self.stiffness_func = stiffness_func
|
|
self.constant_stiffness = constant_stiffness
|
|
self.current_stiffness = 0.0
|
|
|
|
self.vector = self.calc_r_vector()
|
|
self.length = self.calc_length()
|
|
self.unit_vector: list[float] = self.calc_r_unit_vector()
|
|
self.spring_force_scalar: float = 0.0
|
|
self.spring_force: list[float] = self.calc_spring_force()
|
|
|
|
|
|
self.parent_joint.add_spring(self)
|
|
self.child_joint.add_spring(self)
|
|
|
|
self.sharing_attributes = False
|
|
self.shared_attributes: 'SharedSpring' = None
|
|
|
|
def get_name(self):
|
|
return self.name
|
|
|
|
def get_parent_joint(self):
|
|
return self.parent_joint
|
|
|
|
def get_child_joint(self):
|
|
return self.child_joint
|
|
|
|
def get_spring_force(self):
|
|
return self.spring_force
|
|
|
|
def calc_r_vector(self):
|
|
return self.child_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
|
|
|
def calc_length(self):
|
|
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
|
|
|
def calc_r_unit_vector(self):
|
|
return self.vector / self.length
|
|
|
|
def calc_stiffness(self):
|
|
if self.stiffness_func is not None:
|
|
return self.stiffness_func(self.length, self.unstretched_length)
|
|
else:
|
|
return self.constant_stiffness
|
|
|
|
def calc_spring_force(self):
|
|
'''Positive force is in tension. Aka, the spring PULLS on the other object'''
|
|
self.length = self.calc_length()
|
|
del_length = self.length - self.unstretched_length
|
|
self.current_stiffness = self.calc_stiffness()
|
|
self.spring_force_scalar = del_length * self.current_stiffness
|
|
|
|
self.vector = self.calc_r_vector()
|
|
self.r_unit_vector = self.calc_r_unit_vector()
|
|
self.spring_force = self.spring_force_scalar * self.r_unit_vector
|
|
return self.spring_force
|
|
|
|
def print_attributes(self):
|
|
print(f"Spring: {self.name}")
|
|
print(f"Length: {self.length}")
|
|
print(f"Spring Force: {self.spring_force}")
|
|
|
|
def get_pos(self):
|
|
return self.parent_statespace.get_pos()
|
|
def get_axis_vector(self):
|
|
return self.child_statespace.get_pos() - self.parent_statespace.get_pos()
|
|
|
|
def attach_shared_attributes(self, shared_attributes: 'SharedSpring'):
|
|
if not isinstance(shared_attributes._getvalue(), SharedSpring):
|
|
raise TypeError(f"shared_attributes for Spring {self.name} is not a SharedSpring")
|
|
self.sharing_attributes = True
|
|
self.shared_attributes = shared_attributes
|
|
|
|
self.update_shared_attributes()
|
|
|
|
def update_shared_attributes(self):
|
|
if self.shared_attributes is not None:
|
|
self.shared_attributes.set_child_statespace(self.child_joint.get_statespace())
|
|
self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
|
|
self.shared_attributes.set_spring_force(self.spring_force)
|
|
self.shared_attributes.set_spring_force_scalar(self.spring_force_scalar)
|
|
self.shared_attributes.set_length(self.length)
|
|
self.shared_attributes.set_unstretched_length(self.unstretched_length)
|
|
self.shared_attributes.set_stiffness(self.current_stiffness)
|
|
|
|
def is_same(self, other_spring: 'Spring'):
|
|
if self.name == other_spring.get_name():
|
|
return True
|
|
return False
|
|
|
|
|
|
def is_in_list(self, springs: list['Spring']):
|
|
for j in springs:
|
|
if self.is_same(j):
|
|
return True
|
|
return False
|
|
|
|
'''
|
|
It connects 2 joints.
|
|
1 joint is completely fixed. Cannot move. Ever. It will be referred to as "grounded".
|
|
The other joint is contrained to the grounded joint. The actuator controls the vector displacement <x,y,z> from the grounded joint
|
|
'''
|
|
class RigidActuator():
|
|
def __init__(self, parent_joint: 'Joint', grounded_joint: 'Joint', name: str = "Rigid Actuator", max_length:float=1, min_length:float=0.1, control_code=0):
|
|
self.name = name
|
|
|
|
self.parent_joint = parent_joint
|
|
self.grounded_joint = grounded_joint # Same as child joint
|
|
|
|
self.parent_joint.add_actuator(self)
|
|
self.parent_joint.set_connected_to_rigid_actuator(True)
|
|
self.grounded_joint.add_actuator(self)
|
|
self.grounded_joint.set_connected_to_rigid_actuator(True)
|
|
|
|
self.max_length = max_length
|
|
self.min_length = min_length
|
|
self.control_code = control_code # 0 for pos, 1 for vel, 2 for accel
|
|
|
|
self.calc_r_unit_vector()
|
|
|
|
self.shared_attributes: 'SharedRigidActuator' = None
|
|
|
|
def get_name(self):
|
|
return self.name
|
|
|
|
def get_parent_joint(self):
|
|
return self.parent_joint
|
|
|
|
def get_child_joint(self):
|
|
return self.grounded_joint
|
|
|
|
def is_same(self, other: 'RigidActuator'):
|
|
if self.name == other.get_name():
|
|
return True
|
|
return False
|
|
|
|
def is_in_list(self, actuators: list['RigidActuator']):
|
|
for j in actuators:
|
|
if self.is_same(j):
|
|
return True
|
|
return False
|
|
|
|
def calc_r_vector(self):
|
|
return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
|
|
|
def calc_length(self):
|
|
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
|
|
|
def calc_r_unit_vector(self):
|
|
self.vector = self.calc_r_vector()
|
|
self.length = self.calc_length()
|
|
return self.vector / self.length
|
|
|
|
def get_pos(self):
|
|
return self.parent_joint.get_statespace().get_pos()
|
|
|
|
def get_axis_vector(self):
|
|
return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
|
|
|
def get_r_unit_vector(self):
|
|
return self.calc_r_unit_vector()
|
|
|
|
def update_shared_attributes(self):
|
|
# These 2 lines are used to make sure we set the internal properties correctly
|
|
self.vector = self.calc_r_vector()
|
|
self.length = self.calc_length()
|
|
# END
|
|
if self.shared_attributes is not None:
|
|
self.shared_attributes.set_child_statespace(self.grounded_joint.get_statespace())
|
|
self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
|
|
self.shared_attributes.set_length(self.length)
|
|
|
|
def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuator'):
|
|
if not isinstance(shared_attributes._getvalue(), SharedRigidActuator):
|
|
raise TypeError(f"shared_attributes for RigidActuator {self.name} is not a SharedRigidActuator")
|
|
self.sharing_attributes = True
|
|
self.shared_attributes = shared_attributes
|
|
|
|
self.update_shared_attributes()
|
|
|
|
def set_parent_pos(self, new_pos):
|
|
'''Set the position of the parent joint's statestpace'''
|
|
self.parent_joint.set_state_space_pos(new_pos)
|
|
|
|
def update_from_controller(self):
|
|
if self.control_code == 0: # Controller controls pos
|
|
self.set_pos_to_controlled_pos()
|
|
elif self.control_code == 1: # Controller controls vel
|
|
self.set_vel_to_controlled_vel()
|
|
elif self.control_code == 2: # Controls controls accel
|
|
pass
|
|
|
|
def set_vel_to_controlled_vel(self):
|
|
controlled_vel = self.shared_attributes.get_vel()
|
|
self.parent_joint.set_state_space_vel(controlled_vel)
|
|
|
|
def set_pos_to_controlled_pos(self):
|
|
controlled_pos = self.shared_attributes.get_pos()
|
|
self.parent_joint.set_state_space_pos(controlled_pos)
|
|
|
|
|
|
class SharedPhysicsElement():
|
|
def __init__(self) -> None:
|
|
self.name: str = None
|
|
self.statespace: StateSpace = None
|
|
|
|
self.connected_to_plotter: bool = False
|
|
self.connected_to_visualization: bool = False
|
|
self.connected_to_sensor: bool = False
|
|
self.connected_to_controller: bool = False
|
|
|
|
def set_name(self, name:str) -> None:
|
|
self.name = name
|
|
def set_statespace(self, statespace:StateSpace) -> None:
|
|
self.statespace = statespace
|
|
def set_connected_to_plotter(self, state: bool) -> None:
|
|
self.connected_to_plotter = state
|
|
def set_connected_to_visualization(self, state: bool) -> None:
|
|
self.connected_to_visualization = state
|
|
def set_connected_to_sensor(self, state: bool) -> None:
|
|
self.connected_to_sensor = state
|
|
def set_connected_to_controller(self, state: bool) -> None:
|
|
self.connected_to_controller = state
|
|
|
|
def get_name(self) -> str:
|
|
return self.name
|
|
def get_statespace(self) -> StateSpace:
|
|
return self.statespace
|
|
def get_pos(self) -> List[float]:
|
|
return self.statespace.get_pos()
|
|
def get_vel(self) -> List[float]:
|
|
return self.statespace.get_vel()
|
|
def get_accel(self) -> List[float]:
|
|
return self.statespace.get_accel()
|
|
def get_force(self) -> List[float]:
|
|
return self.statespace.get_force()
|
|
def get_connected_to_plotter(self) -> bool:
|
|
return self.connected_to_plotter
|
|
def get_connected_to_visualization(self) -> bool:
|
|
return self.connected_to_visualization
|
|
def get_connected_to_sensor(self) -> bool:
|
|
return self.connected_to_sensor
|
|
def get_connected_to_controller(self) -> bool:
|
|
return self.connected_to_controller
|
|
|
|
class SharedJoint(SharedPhysicsElement):
|
|
def _init__(self) -> None:
|
|
super().__init__()
|
|
|
|
class SharedSpring(SharedPhysicsElement):
|
|
'''self.statespace is the statespace of the parent'''
|
|
def _init__(self) -> None:
|
|
super().__init__()
|
|
self.child_statespace: StateSpace = None
|
|
|
|
self.unstretched_length: float = None
|
|
self.length: float = None
|
|
self.stiffness: float = None
|
|
|
|
self.spring_force: float = None
|
|
|
|
def set_parent_statespace(self, statespace:StateSpace) -> None:
|
|
self.statespace = statespace
|
|
def set_child_statespace(self, statespace:StateSpace) -> None:
|
|
self.child_statespace = statespace
|
|
def set_unstretched_length(self, unstretched_length: float) -> None:
|
|
self.unstretched_length = unstretched_length
|
|
def set_length(self, length: float) -> None:
|
|
self.length = length
|
|
def set_stiffness(self, stiffness: float) -> None:
|
|
self.stiffness = stiffness
|
|
def set_spring_force(self, set_spring_force: float) -> None:
|
|
self.spring_force = set_spring_force
|
|
def set_spring_force_scalar(self, scalar_force: float) -> None:
|
|
self.spring_force_scalar = scalar_force
|
|
|
|
def get_parent_statespace(self) -> StateSpace:
|
|
return self.statespace
|
|
def get_child_statespace(self) -> StateSpace:
|
|
return self.child_statespace
|
|
def get_unstretched_length(self) -> float:
|
|
return self.unstretched_length
|
|
def get_length(self) -> float:
|
|
return self.length
|
|
def get_stiffness(self) -> float:
|
|
return self.stiffness
|
|
def get_spring_force(self) -> list[float]:
|
|
return self.spring_force
|
|
def get_spring_force_scalar(self) -> float:
|
|
return self.spring_force_scalar
|
|
def get_axis_vector(self) -> List[float]:
|
|
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
|
|
|
|
|
class SharedRigidActuator(SharedPhysicsElement):
|
|
'''self.statespace is the statespace of the parent'''
|
|
def _init__(self) -> None:
|
|
super().__init__()
|
|
self.child_statespace: StateSpace = None
|
|
|
|
self.length: float = None
|
|
|
|
def set_parent_statespace(self, statespace:StateSpace) -> None:
|
|
self.statespace = statespace
|
|
def set_parent_statespace_pos(self, new):
|
|
self.statespace.set_pos(new)
|
|
def set_parent_statespace_vel(self, new):
|
|
self.statespace.set_vel(new)
|
|
def set_child_statespace(self, statespace:StateSpace) -> None:
|
|
self.child_statespace = statespace
|
|
def set_length(self, length: float) -> None:
|
|
self.length = length
|
|
|
|
def get_parent_statespace(self) -> StateSpace:
|
|
return self.statespace
|
|
def get_child_statespace(self) -> StateSpace:
|
|
return self.child_statespace
|
|
def get_length(self) -> float:
|
|
return self.length
|
|
def get_axis_vector(self) -> List[float]:
|
|
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
|
def get_r_unit_vector(self):
|
|
return self.calc_r_unit_vector()
|
|
def calc_r_vector(self):
|
|
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
|
def calc_length(self):
|
|
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
|
def calc_r_unit_vector(self):
|
|
self.vector = self.calc_r_vector()
|
|
self.length = self.calc_length()
|
|
return self.vector / self.length |