RustSynx/2dof_arm.xml Secret

## 2dof_arm.xml
<mujoco model="2dof_arm">
    <asset>
        <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1="0 0 0" rgb2="1 1 1"/>
        <material name="grid_mat" texture="grid" texrepeat="10 10" texuniform="true"/>
    </asset>
    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" material="grid_mat"/>
        <body name="base" pos="0 0 0">
            <geom type="cylinder" size="0.05 0.05" rgba="0.2 0.2 0.2 1"/>
            <body name="link1" pos="0 0 0.05">
                <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" damping="50"/>
                <geom type="capsule" size="0.01" fromto="0 0 0 0.2 0 0" rgba="1 0 0 1"/>
                <body name="link2" pos="0.2 0 0">
                    <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" damping="50"/>
                    <geom type="sphere" size="0.015" rgba="1 1 1 1"/>
                    <geom type="capsule" size="0.01" fromto="0 0 0 0.2 0 0" rgba="0 1 0 1"/>
                </body>
            </body>
        </body>
    </worldbody>
    <actuator>
        <position name="m1" joint="joint1" kp="100" kv="10" forcerange="-50 50"/>
        <position name="m2" joint="joint2" kp="100" kv="10" forcerange="-50 50"/>
    </actuator>
</mujoco>

## 2dof_robot_simulation.cpp
#include <GLFW/glfw3.h>
#include <cstdio>
#include <mujoco/mujoco.h>
#include <thread>

mjModel *m = NULL;
mjData *d = NULL;
mjvCamera cam;
mjvOption opt;
mjvScene scn;
mjrContext con;
bool button_left = false;
bool button_right = false;
bool button_middle = false;
double lastx = 0;
double lasty = 0;

void mouse_button(GLFWwindow* window, int button, int action, int mods) {
  button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS);
  button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS);
  button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS);

  glfwGetCursorPos(window, &lastx, &lasty);
}

void mouse_move(GLFWwindow* window, double xpos, double ypos) {
  if (!button_left && !button_right && !button_middle) return;

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

  int width, height;
  glfwGetWindowSize(window, &width, &height);

  mjtMouse action;
  if (button_right)      action = mjMOUSE_MOVE_V;
  else if (button_left)  action = mjMOUSE_ROTATE_V;
  else                   action = mjMOUSE_ZOOM;

  mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
}

void scroll(GLFWwindow* window, double xoffset, double yoffset) {
  mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
}

int main() {
  if (!glfwInit())
    return 1;
  char error[1000] = "Could not load binary model";
  std::string modelPath = std::string(SOURCE_DIR) + "/2dof_arm.xml";
  m = mj_loadXML(modelPath.c_str(), 0, error, 1000);
  d = mj_makeData(m);

  GLFWwindow *window =
      glfwCreateWindow(1200, 900, "2-DOF Arm Simulation", NULL, NULL);
  glfwMakeContextCurrent(window);
  mjv_defaultCamera(&cam);
  mjv_defaultOption(&opt);
  mjv_defaultScene(&scn);
  mjr_defaultContext(&con);

  mjv_makeScene(m, &scn, 1000);
  mjr_makeContext(m, &con, mjFONTSCALE_150);

  bool is_paused = true;

  while (!glfwWindowShouldClose(window)) {
    if (glfwGetKey(window, GLFW_KEY_SPACE) == GLFW_PRESS) {
      is_paused = false;
    }

    mj_step(m, d);
    if (!is_paused) {
      d->ctrl[0] = 0.59;
      d->ctrl[1] = 1.318;
      mj_step(m, d);
    }

    mjrRect viewport = {0, 0, 0, 0};
    glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
    mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
    mjr_render(viewport, &scn, &con);

    glfwSetMouseButtonCallback(window, mouse_button);
    glfwSetCursorPosCallback(window, mouse_move);
    glfwSetScrollCallback(window, scroll);
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

  mj_deleteData(d);
  mj_deleteModel(m);
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  return 0;
}

## CMakeLists.txt
cmake_minimum_required(VERSION 4.1)
project(MujocoSim)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS ON)

include(FetchContent)
FetchContent_Declare(
        mujoco
        GIT_REPOSITORY https://github.com/google-deepmind/mujoco.git
        GIT_TAG        3.4.0
)
FetchContent_MakeAvailable(mujoco)

add_executable(main main.cpp)

target_link_libraries(main
        mujoco::mujoco
        glfw
        "-framework Cocoa"
        "-framework IOKit"
        "-framework OpenGL"
)

target_compile_definitions(main PRIVATE
        SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}"
)

## ik_plot.py
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches

def calculate_and_plot_full_ik(x_target, y_target, L1=20, L2=20):
    r_sq = x_target**2 + y_target**2
    r = np.sqrt(r_sq)

    phi = np.arctan2(y_target, x_target)

    cos_alpha = (L1**2 + r_sq - L2**2) / (2 * L1 * r)
    cos_alpha = np.clip(cos_alpha, -1.0, 1.0)
    alpha = np.arccos(cos_alpha)

    theta1 = phi - alpha

    cos_theta2_internal = (L1**2 + L2**2 - r_sq) / (2 * L1 * L2)
    theta2_internal = np.arccos(np.clip(cos_theta2_internal, -1.0, 1.0))

    theta2_actual = np.pi - theta2_internal

    x_elbow = L1 * np.cos(theta1)
    y_elbow = L1 * np.sin(theta1)

    fig, ax = plt.subplots(figsize=(10, 10))

    ax.plot([0, 40], [0, 0], 'k-', linewidth=1, alpha=0.5)
    ax.plot([0, x_target], [0, y_target], 'g-', label='$r$', alpha=0.6)
    ax.plot([0, x_elbow], [0, y_elbow], 'y-', linewidth=4, label='$L_1$', alpha=0.8)
    ax.plot([x_elbow, x_target], [y_elbow, y_target], 'p-', linewidth=4, label='$L_2$', alpha=0.6)
    ax.plot(0, 0, 'ko', markersize=10, label='Base')
    ax.plot(x_elbow, y_elbow, 'bo', markersize=8)
    ax.plot(x_target, y_target, 'ro', markersize=12, label='Target')

    # Phi
    arc_phi = patches.Arc((0,0), 14, 14, angle=0, theta1=0, theta2=np.degrees(phi),
                          color='green', linewidth=1.5, linestyle='--')
    ax.add_patch(arc_phi)
    ax.text(7, 3, r'$\phi$', color='green', fontsize=12)

    # Theta 1
    arc_theta1 = patches.Arc((0,0), 18, 18, angle=0, theta1=0, theta2=np.degrees(theta1),
                             color='blue', linewidth=2.5)
    ax.add_patch(arc_theta1)
    ax.text(9, 3, r'$\theta_1$', color='blue', fontsize=14, fontweight='bold')

    # Alpha
    arc_alpha = patches.Arc((0,0), 16, 16, angle=0, theta1=np.degrees(theta1), theta2=np.degrees(phi),
                            color='red', linewidth=1.5)
    ax.add_patch(arc_alpha)
    ax.text(6, 7, r'$\alpha$', color='red', fontsize=12)

    # Theta 2
    link1_angle_deg = np.degrees(theta1)
    start_angle_t2 = link1_angle_deg + 180 - np.degrees(theta2_internal)
    arc_theta2_int = patches.Arc((x_elbow, y_elbow), 10, 10, angle=0,
                                 theta1=start_angle_t2, theta2=link1_angle_deg + 180,
                                 color='purple', linewidth=2)
    ax.add_patch(arc_theta2_int)
    ax.text(x_elbow-3, y_elbow+1, r'$\theta_{2}$', color='purple', fontsize=12)


    ax.set_xlim(-5, 35)
    ax.set_ylim(-5, 35)
    ax.set_aspect('equal')
    ax.grid(True, linestyle=':', alpha=0.6)
    ax.set_title(f'IK Visualization \nTarget: ({x_target}, {y_target})')
    ax.set_xlabel('X (cm)')
    ax.set_ylabel('Y (cm)')
    ax.legend(loc='lower right')
    plt.show()

calculate_and_plot_full_ik(10, 30)
	<mujoco model="2dof_arm">
	<asset>
	<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1="0 0 0" rgb2="1 1 1"/>
	<material name="grid_mat" texture="grid" texrepeat="10 10" texuniform="true"/>
	</asset>
	<worldbody>
	<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
	<geom type="plane" size="1 1 0.1" material="grid_mat"/>
	<body name="base" pos="0 0 0">
	<geom type="cylinder" size="0.05 0.05" rgba="0.2 0.2 0.2 1"/>
	<body name="link1" pos="0 0 0.05">
	<joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" damping="50"/>
	<geom type="capsule" size="0.01" fromto="0 0 0 0.2 0 0" rgba="1 0 0 1"/>
	<body name="link2" pos="0.2 0 0">
	<joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" damping="50"/>
	<geom type="sphere" size="0.015" rgba="1 1 1 1"/>
	<geom type="capsule" size="0.01" fromto="0 0 0 0.2 0 0" rgba="0 1 0 1"/>
	</body>
	</body>
	</body>
	</worldbody>
	<actuator>
	<position name="m1" joint="joint1" kp="100" kv="10" forcerange="-50 50"/>
	<position name="m2" joint="joint2" kp="100" kv="10" forcerange="-50 50"/>
	</actuator>
	</mujoco>
	#include <GLFW/glfw3.h>
	#include <cstdio>
	#include <mujoco/mujoco.h>
	#include <thread>

	mjModel *m = NULL;
	mjData *d = NULL;
	mjvCamera cam;
	mjvOption opt;
	mjvScene scn;
	mjrContext con;
	bool button_left = false;
	bool button_right = false;
	bool button_middle = false;
	double lastx = 0;
	double lasty = 0;

	void mouse_button(GLFWwindow* window, int button, int action, int mods) {
	button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS);
	button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS);
	button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS);

	glfwGetCursorPos(window, &lastx, &lasty);
	}

	void mouse_move(GLFWwindow* window, double xpos, double ypos) {
	if (!button_left && !button_right && !button_middle) return;

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

	int width, height;
	glfwGetWindowSize(window, &width, &height);

	mjtMouse action;
	if (button_right) action = mjMOUSE_MOVE_V;
	else if (button_left) action = mjMOUSE_ROTATE_V;
	else action = mjMOUSE_ZOOM;

	mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
	}

	void scroll(GLFWwindow* window, double xoffset, double yoffset) {
	mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
	}

	int main() {
	if (!glfwInit())
	return 1;
	char error[1000] = "Could not load binary model";
	std::string modelPath = std::string(SOURCE_DIR) + "/2dof_arm.xml";
	m = mj_loadXML(modelPath.c_str(), 0, error, 1000);
	d = mj_makeData(m);

	GLFWwindow *window =
	glfwCreateWindow(1200, 900, "2-DOF Arm Simulation", NULL, NULL);
	glfwMakeContextCurrent(window);
	mjv_defaultCamera(&cam);
	mjv_defaultOption(&opt);
	mjv_defaultScene(&scn);
	mjr_defaultContext(&con);

	mjv_makeScene(m, &scn, 1000);
	mjr_makeContext(m, &con, mjFONTSCALE_150);

	bool is_paused = true;

	while (!glfwWindowShouldClose(window)) {
	if (glfwGetKey(window, GLFW_KEY_SPACE) == GLFW_PRESS) {
	is_paused = false;
	}

	mj_step(m, d);
	if (!is_paused) {
	d->ctrl[0] = 0.59;
	d->ctrl[1] = 1.318;
	mj_step(m, d);
	}

	mjrRect viewport = {0, 0, 0, 0};
	glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
	mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
	mjr_render(viewport, &scn, &con);

	glfwSetMouseButtonCallback(window, mouse_button);
	glfwSetCursorPosCallback(window, mouse_move);
	glfwSetScrollCallback(window, scroll);
	glfwSwapBuffers(window);
	glfwPollEvents();
	}

	mj_deleteData(d);
	mj_deleteModel(m);
	mjr_freeContext(&con);
	mjv_freeScene(&scn);
	return 0;
	}
	cmake_minimum_required(VERSION 4.1)
	project(MujocoSim)

	set(CMAKE_CXX_STANDARD 17)
	set(CMAKE_CXX_STANDARD_REQUIRED ON)

	set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS ON)

	include(FetchContent)
	FetchContent_Declare(
	mujoco
	GIT_REPOSITORY https://github.com/google-deepmind/mujoco.git
	GIT_TAG 3.4.0
	)
	FetchContent_MakeAvailable(mujoco)

	add_executable(main main.cpp)

	target_link_libraries(main
	mujoco::mujoco
	glfw
	"-framework Cocoa"
	"-framework IOKit"
	"-framework OpenGL"
	)

	target_compile_definitions(main PRIVATE
	SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}"
	)
	import numpy as np
	import matplotlib.pyplot as plt
	import matplotlib.patches as patches

	def calculate_and_plot_full_ik(x_target, y_target, L1=20, L2=20):
	r_sq = x_target2 + y_target2
	r = np.sqrt(r_sq)

	phi = np.arctan2(y_target, x_target)

	cos_alpha = (L12 + r_sq - L22) / (2 * L1 * r)
	cos_alpha = np.clip(cos_alpha, -1.0, 1.0)
	alpha = np.arccos(cos_alpha)

	theta1 = phi - alpha

	cos_theta2_internal = (L12 + L22 - r_sq) / (2 * L1 * L2)
	theta2_internal = np.arccos(np.clip(cos_theta2_internal, -1.0, 1.0))

	theta2_actual = np.pi - theta2_internal

	x_elbow = L1 * np.cos(theta1)
	y_elbow = L1 * np.sin(theta1)

	fig, ax = plt.subplots(figsize=(10, 10))

	ax.plot([0, 40], [0, 0], 'k-', linewidth=1, alpha=0.5)
	ax.plot([0, x_target], [0, y_target], 'g-', label='$r$', alpha=0.6)
	ax.plot([0, x_elbow], [0, y_elbow], 'y-', linewidth=4, label='$L_1$', alpha=0.8)
	ax.plot([x_elbow, x_target], [y_elbow, y_target], 'p-', linewidth=4, label='$L_2$', alpha=0.6)
	ax.plot(0, 0, 'ko', markersize=10, label='Base')
	ax.plot(x_elbow, y_elbow, 'bo', markersize=8)
	ax.plot(x_target, y_target, 'ro', markersize=12, label='Target')

	# Phi
	arc_phi = patches.Arc((0,0), 14, 14, angle=0, theta1=0, theta2=np.degrees(phi),
	color='green', linewidth=1.5, linestyle='--')
	ax.add_patch(arc_phi)
	ax.text(7, 3, r'$\phi$', color='green', fontsize=12)

	# Theta 1
	arc_theta1 = patches.Arc((0,0), 18, 18, angle=0, theta1=0, theta2=np.degrees(theta1),
	color='blue', linewidth=2.5)
	ax.add_patch(arc_theta1)
	ax.text(9, 3, r'$\theta_1$', color='blue', fontsize=14, fontweight='bold')

	# Alpha
	arc_alpha = patches.Arc((0,0), 16, 16, angle=0, theta1=np.degrees(theta1), theta2=np.degrees(phi),
	color='red', linewidth=1.5)
	ax.add_patch(arc_alpha)
	ax.text(6, 7, r'$\alpha$', color='red', fontsize=12)

	# Theta 2
	link1_angle_deg = np.degrees(theta1)
	start_angle_t2 = link1_angle_deg + 180 - np.degrees(theta2_internal)
	arc_theta2_int = patches.Arc((x_elbow, y_elbow), 10, 10, angle=0,
	theta1=start_angle_t2, theta2=link1_angle_deg + 180,
	color='purple', linewidth=2)
	ax.add_patch(arc_theta2_int)
	ax.text(x_elbow-3, y_elbow+1, r'$\theta_{2}$', color='purple', fontsize=12)


	ax.set_xlim(-5, 35)
	ax.set_ylim(-5, 35)
	ax.set_aspect('equal')
	ax.grid(True, linestyle=':', alpha=0.6)
	ax.set_title(f'IK Visualization \nTarget: ({x_target}, {y_target})')
	ax.set_xlabel('X (cm)')
	ax.set_ylabel('Y (cm)')
	ax.legend(loc='lower right')
	plt.show()

	calculate_and_plot_full_ik(10, 30)