Open SwapnilSonawane894 opened 1 month ago
Sure, can you give me more info about what are you working on, and how i can be of help?
Sir I am working on self-balancing bot I am trying to balance the bot using LQR control technique I am using Coppelia Sim for it with python. I am facing problems for finding the LQR gain tried all possible solution could please help 🥺
@gauravsethia08 Please reply, Please help me with these project.
Could you provide a link to your code base and mention whats issue you are facing?
@gauravsethia08 ok sir
import numpy as np
import time
def sysCall_init():
global velocity, yaw_velocity, last_key_time, radius
sim = require('sim')
math = require('math')
# Initialization
self.body = sim.getObject("/body")
self.rightJoint = sim.getObject("/right_joint")
self.leftJoint = sim.getObject("/left_joint")
self.floor = sim.getObject("/Floor")
self.K = np.array([1.4142135623731116, 1.6324576461398326, 0.8958387217853694, 0.01639766421577393])
self.accel = 0.0
self.turn = 0.0
self.damping_factor = 2.5 # Adjust as needed
velocity = 0.0 # Initialize global velocity
yaw_velocity = 0.0 # Initialize global yaw velocity
radius = 0.0215 # Radius of the wheel (m)
last_key_time = time.time() # Initialize last key press time
def saturate(value, min_value, max_value):
return max(min(value, max_value), min_value)
def control():
global velocity, yaw_velocity, last_key_time
message, data, data2 = sim.getSimulatorMessage()
current_time = time.time()
if message == sim.message_keypress:
if current_time - last_key_time > 0.1: # Adjust debounce time as needed
if data[0] == 2007: # forward up arrow
velocity = 20.0 # Set speed forward
elif data[0] == 2008: # backward down arrow
velocity = -20.0 # Set speed backward
elif data[0] == 2009: # left arrow key (yaw left)
yaw_velocity = 2.0 # Set yaw left and reduced impact
elif data[0] == 2010: # right arrow key (yaw right)
yaw_velocity = -2.0 # Set yaw right and reduced impact
last_key_time = current_time
else:
velocity = 0.0 # Stop when no key pressed
yaw_velocity = 0.0
def sysCall_actuation():
global velocity, yaw_velocity, radius
# Get the current state
objectMat = sim.getObjectMatrix(self.body, self.floor)
unitX = objectMat[1:10:4]
unitY = objectMat[2:11:4]
unitZ = objectMat[3:12:4]
rollDeg = np.degrees(np.arctan2(unitX[2], unitZ[2])) # Roll angle
pErr = np.degrees(np.arctan2(-unitY[2], unitZ[2])) # Pitch angle
xErr = sim.getObjectPosition(self.body, self.floor)[1] # Position error
# Get velocity (linear and angular) from simulation
linearVelocity, angularVelocity = sim.getObjectVelocity(self.body, self.floor)
# State vector [position, velocity, angle, angular velocity]
state = np.array([xErr, linearVelocity[1], rollDeg, angularVelocity[2]])
# Compute control input using LQR with damping factor
u = -np.dot(self.K, state) * self.damping_factor
# Convert torque to angular velocity
angular_velocity_right = (u + self.accel + velocity - yaw_velocity) / radius
angular_velocity_left = (u + self.accel + velocity + yaw_velocity) / radius
# Cap the velocities to prevent excessive values
angular_velocity_right = saturate(angular_velocity_right, -10, 10) # Adjust limits as needed
angular_velocity_left = saturate(angular_velocity_left, -10, 10) # Adjust limits as needed
# Apply control input to the joints
sim.setJointTargetVelocity(self.rightJoint, angular_velocity_right)
sim.setJointTargetVelocity(self.leftJoint, angular_velocity_left)
def sysCall_sensing():
control()
def sysCall_cleanup():
pass
just paseted here, i am not sure my lqr gain is correct or not, could you please help in thes for finding the gain for smooth balance in the bot ( i am using coppelia simulation for these )
@gauravsethia08 Please help in me in these
Sir i want help in my blancing bot could you please help me