Skip to content

Commit

Permalink
asdf
Browse files Browse the repository at this point in the history 
asdf
  • Loading branch information
@jeongsukchul
jeongsukchul authored Nov 27, 2023
0 parents commit dc1cb08
Show file tree
Hide file tree
Showing 4 changed files with 468 additions and 0 deletions.
114 changes: 114 additions & 0 deletions MUJOCO_LOG.TXT
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
Sun Nov 26 17:38:09 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 3.5340.

Sun Nov 26 17:38:20 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 6.2580.

Sun Nov 26 17:40:15 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 6.2580.

Sun Nov 26 18:03:36 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 6.3640.

Sun Nov 26 19:36:08 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 6.3640.

Sun Nov 26 19:37:11 2023
WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 6.3640.

Sun Nov 26 19:37:24 2023
WARNING: Nan, Inf or huge value in QACC at DOF 1. The simulation is unstable. Time = 0.7120.

Sun Nov 26 19:38:12 2023
WARNING: Nan, Inf or huge value in QACC at DOF 1. The simulation is unstable. Time = 0.7120.

Sun Nov 26 19:38:48 2023
WARNING: Nan, Inf or huge value in QACC at DOF 1. The simulation is unstable. Time = 0.7120.

Sun Nov 26 19:40:21 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:44:01 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:46:58 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:47:12 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:48:50 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:49:20 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:54:18 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:54:29 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:55:33 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 0.1040.

Sun Nov 26 19:56:27 2023
WARNING: Nan, Inf or huge value in QACC at DOF 5. The simulation is unstable. Time = 1.6380.

Sun Nov 26 19:56:51 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 12.5560.

Sun Nov 26 19:57:04 2023
WARNING: Nan, Inf or huge value in QACC at DOF 5. The simulation is unstable. Time = 1.9580.

Sun Nov 26 19:59:53 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 1.9700.

Sun Nov 26 20:05:43 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 6.7560.

Sun Nov 26 23:27:24 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 6.7560.

Sun Nov 26 23:34:43 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 1.7460.

Sun Nov 26 23:35:50 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 15.7960.

Sun Nov 26 23:43:16 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 3.1640.

Sun Nov 26 23:44:13 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 15.7960.

Sun Nov 26 23:54:45 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 11.5060.

Sun Nov 26 23:57:11 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 4.1860.

Sun Nov 26 23:58:34 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 16.9600.

Sun Nov 26 23:59:00 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 16.9600.

Mon Nov 27 00:00:52 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 16.9600.

Mon Nov 27 00:14:34 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 1.5760.

Mon Nov 27 01:25:35 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 9.2420.

Mon Nov 27 02:55:16 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 11.5060.

Mon Nov 27 03:17:15 2023
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 3.3000.

Mon Nov 27 13:00:19 2023
WARNING: Nan, Inf or huge value in QACC at DOF 4. The simulation is unstable. Time = 24.8400.

225 changes: 225 additions & 0 deletions drone.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,225 @@
import os

import mujoco as mj
import numpy as np
from mujoco.glfw import glfw
from scipy.spatial.transform import Rotation as R

xml_path = 'drone.xml' # xml file (assumes this is in the same folder as this file)
simend = 200 # simulation time
print_camera_config = 0 # set to 1 to print camera config
# this is useful for initializing view of the model)

# For callback functions
button_left = False
button_middle = False
button_right = False
lastx = 0
lasty = 0

def init_controller(model, data):
# initialize the controller here. This function is called once, in the beginning
pass


def controller(model, data):
# put the controller here. This function is called inside the simulation.
pass


def keyboard(window, key, scancode, act, mods):
if act == glfw.PRESS and key == glfw.KEY_BACKSPACE:
mj.mj_resetData(model, data)
mj.mj_forward(model, data)


def mouse_button(window, button, act, mods):
# update button state
global button_left
global button_middle
global button_right

button_left = (glfw.get_mouse_button(
window, glfw.MOUSE_BUTTON_LEFT) == glfw.PRESS)
button_middle = (glfw.get_mouse_button(
window, glfw.MOUSE_BUTTON_MIDDLE) == glfw.PRESS)
button_right = (glfw.get_mouse_button(
window, glfw.MOUSE_BUTTON_RIGHT) == glfw.PRESS)

# update mouse position
glfw.get_cursor_pos(window)



def mouse_move(window, xpos, ypos):
# compute mouse displacement, save
global lastx
global lasty
global button_left
global button_middle
global button_right

dx = xpos - lastx
dy = ypos - lasty
lastx = xpos
lasty = ypos

# no buttons down: nothing to do
if (not button_left) and (not button_middle) and (not button_right):
return

# get current window size
width, height = glfw.get_window_size(window)

# get shift key state
PRESS_LEFT_SHIFT = glfw.get_key(
window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS
PRESS_RIGHT_SHIFT = glfw.get_key(
window, glfw.KEY_RIGHT_SHIFT) == glfw.PRESS
mod_shift = (PRESS_LEFT_SHIFT or PRESS_RIGHT_SHIFT)

# determine action based on mouse button
if button_right:
if mod_shift:
action = mj.mjtMouse.mjMOUSE_MOVE_H
else:
action = mj.mjtMouse.mjMOUSE_MOVE_V
elif button_left:
if mod_shift:
action = mj.mjtMouse.mjMOUSE_ROTATE_H
else:
action = mj.mjtMouse.mjMOUSE_ROTATE_V
else:
action = mj.mjtMouse.mjMOUSE_ZOOM

mj.mjv_moveCamera(model, action, dx / height,
dy / height, scene, cam)


def scroll(window, xoffset, yoffset):
action = mj.mjtMouse.mjMOUSE_ZOOM
mj.mjv_moveCamera(model, action, 0.0, -0.05 *
yoffset, scene, cam)


# get the full path
dirname = os.path.dirname(__file__)
abspath = os.path.join(dirname + "/" + xml_path)
xml_path = abspath

# MuJoCo data structures
model = mj.MjModel.from_xml_path(xml_path) # MuJoCo model
data = mj.MjData(model) # MuJoCo data
cam = mj.MjvCamera() # Abstract camera
opt = mj.MjvOption() # visualization options

# Init GLFW, create window, make OpenGL context current, request v-sync
glfw.init()
window = glfw.create_window(1200, 900, "Demo", None, None)
glfw.make_context_current(window)
glfw.swap_interval(1)

# initialize visualization data structures
mj.mjv_defaultCamera(cam)
mj.mjv_defaultOption(opt)
scene = mj.MjvScene(model, maxgeom=10000)
context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value)

# install GLFW mouse and keyboard callbacks
glfw.set_key_callback(window, keyboard)
glfw.set_cursor_pos_callback(window, mouse_move)
glfw.set_mouse_button_callback(window, mouse_button)
glfw.set_scroll_callback(window, scroll)

# cam.azimuth = -90.68741727466428;
# cam.elevation = -2.8073894766455036;
cam.distance = 10
cam.lookat = np.array([0.0, 0.0, 3.0])

# initialize the controller
init_controller(model, data)
prev_x = data.xpos[1]
time_prev = data.time
# set the controller
mj.set_mjcb_control(controller)

def rotation_matrix_to_euler(rot_matrix):
r = R.from_matrix(rot_matrix)
euler = r.as_euler('xyz', degrees=True)
euler = euler/180*np.pi
return euler

def angvel_to_euler_rate(w, angle):
J = [[1,np.sin(angle[0])*np.tan(angle[1]),np.cos(angle[0])*np.tan(angle[1])],
[0, +np.cos(angle[0]), -np.sin(angle[0])],
[0, np.sin(angle[0])/np.cos(angle[1]), np.cos(angle[0])/np.cos(angle[1])]]
return J @ w
m =2.5
l = .2
rho = 0.1
inertia = np.array([2,2,3])
x_d = [0,1,2,0,0,0]
idx=0
while not glfw.window_should_close(window):
while (data.time - time_prev < 1.0 / 60.0):

r_mat = data.xmat[1].reshape(3, 3)
angle = rotation_matrix_to_euler(r_mat)
x_dot = np.zeros(6)
mj.mj_objectVelocity(model, data, mj.mjtObj.mjOBJ_BODY,
1, x_dot, 0)
x_dot[0:3] = angvel_to_euler_rate( x_dot[0:3],angle)
x_dot = np.array([x_dot[3],x_dot[4],x_dot[5],x_dot[0],x_dot[1],x_dot[2]])
ftou = np.array([1/m*np.array([1, 1, 1, 1]),
l/inertia[0]*np.array([0, 1, 0, -1]),
l/inertia[1] *np.array([-1,0,1,0]),
rho/inertia[2] *np.array([1,-1,1,-1])])
if r_mat[2,2]==0 and r_mat[1,2]==0:
r_mat = np.eye(3)
K=np.diag([1,1,0.7,0.5,0.5,1])
C= np.diag([5,10,1,1,1,1])

x = np.concatenate((data.xpos[1],angle))
e= x-x_d
s= x_dot + K @ e
G_ = np.array([[0,0,1,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1],[1,0,0,0,0,0],[0,1,0,0,0,0]])
u = G_ @ ([0,0,9.81,0,0,0] -K @ x_dot -0.1*np.sign(s))
f = np.array(np.linalg.inv(ftou) @ u[0:4])
data.ctrl[0:4] = f
idx+=1
if idx%1000==0:
print("pos", data.xpos[1])
print("x_dot",x_dot)
print("s",s)
print("e",e)
print("u",u)
print("f",f)
print(1/4*m*9.81)
print(np.linalg.inv(ftou))
mj.mj_step(model, data)
if (data.time >= simend):
break

# get framebuffer viewport
viewport_width, viewport_height = glfw.get_framebuffer_size(
window)
viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)

time_prev = data.time
# print camera configuration (help to initialize the view)
if (print_camera_config == 1):
print('cam.azimuth =', cam.azimuth, ';', 'cam.elevation =', cam.elevation, ';', 'cam.distance = ', cam.distance)
print('cam.lookat =np.array([', cam.lookat[0], ',', cam.lookat[1], ',', cam.lookat[2], '])')

# Update scene and render
mj.mjv_updateScene(model, data, opt, None, cam,
mj.mjtCatBit.mjCAT_ALL.value, scene)
mj.mjr_render(viewport, scene, context)

# swap OpenGL buffers (blocking call due to v-sync)
glfw.swap_buffers(window)

# process pending GUI events, call GLFW callbacks
glfw.poll_events()

glfw.terminate()
43 changes: 43 additions & 0 deletions drone.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
<mujoco>
<option gravity="0 0 -9.81"/>
<asset>
<texture builtin="gradient" type="skybox" height="100" width="100" rgb1="1 1 1" rgb2="0 0 0"/>
<texture name="texgeom" builtin="flat" height="1278" mark="cross" markrgb="1 1 1" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture name="texplane" builtin="checker" height="100" width="100" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 1" rgba=".9 0 0 1"/>
<body pos="0 0 2" euler="0 0 0" name="body">
<geom name="core_geom" type="box" pos="0 0 0" euler="0 0 45" size="0.0707 0.0707 .02" rgba=".3 .3 .8 1"/>

<geom name="arm_front_left" type="box" pos=".1 0 0" size=".1 .005 .005" euler="0 0 0" rgba=".3 .3 .8 1"/>
<geom name="arm_front_right" type="box" pos="0 .1 0" size=".1 .005 .005" euler="0 0 90" rgba=".3 .3 .8 1"/>
<geom name="arm_back_right" type="box" pos="-.1 0 0" size=".1 .005 .005" euler="0 0 0" rgba=".3 .3 .8 1"/>
<geom name="arm_back_left" type="box" pos="0 -.1 0" size=".1 .005 .005" euler="0 0 90" rgba=".3 .3 .8 1"/>

<geom name="thruster_front_left" type="cylinder" pos=".2 0 .0075" size=".05 .0025" quat="1 0 0 0" rgba=".3 .8 .3 0.6"/>
<geom name="thruster_front_right" type="cylinder" pos="0 .2 .0075" size=".05 .0025" quat="1 0 0 0" rgba=".3 .8 .3 0.6"/>
<geom name="thruster_back_right" type="cylinder" pos="-.2 0 .0075" size=".05 .0025" quat="1 0 0 0" rgba=".3 .8 .3 0.6"/>
<geom name="thruster_back_left" type="cylinder" pos="0 -.2 .0075" size=".05 .0025" quat="1 0 0 0" rgba=".3 .8 .3 0.6"/>
<inertial pos="0 0 0" mass="2.5" diaginertia="2 2 3" />
<joint name="root" type="free" damping="0" armature="0" pos="0 0 0" limited="false"/>
<site name="u1" pos =".2 0 .0075"/>
<site name="u2" pos ="0 0.2 .0075" />
<site name="u3" pos ="-.2 0 .0075" />
<site name="u4" pos ="0 -.2 .0075" />
</body>
</worldbody>
<actuator>
<motor ctrllimited="true" ctrlrange="0 1000" site="u1" gear="0 0 1 0 0 0.1"/>
<motor ctrllimited="true" ctrlrange="0 1000" site="u2" gear="0 0 1 0 0 -0.1"/>

<motor ctrllimited="true" ctrlrange="0 1000" site="u3" gear="0 0 1 0 0 0.1"/>

<motor ctrllimited="true" ctrlrange="0 1000" site="u4" gear="0 0 1 0 0 -0.1"/>
<!-- <motor name="n1" joint="n1" gear="1"/>-->
<!-- <motor name="n2" joint="n2" gear="1"/>-->
</actuator>
</mujoco>
Loading

0 comments on commit dc1cb08

Please sign in to comment.