Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Aloha does not showing up collsion property and not colliding with objeject #107

Open
Akumar201 opened this issue Oct 29, 2024 · 3 comments
Labels
bug Something isn't working

Comments

@Akumar201
Copy link

Which model is the issue affecting?

  1. I am using aloha, using [scene.xml ](https://github.com/google-deepmind/mujoco_menagerie/blob/main/aloha/scene.xml)
    What is the issue?
  2. The arm is going through the table and not colliding with the table

Is there any additional context you can provide (e.g., a spec sheet or a URDF to show a value mismatch)?
my code that I am using looks like
I am using mujoco 3.2.4


!/usr/bin/env python3

import mujoco
from mujoco import viewer
import numpy as np
import time
import rospy
from arm_control.msg import JointControl  
import cv2

model_path = "model/aloha/scene.xml"  # Replace with your MuJoCo XML model file

m = mujoco.MjModel.from_xml_path(model_path)
d = mujoco.MjData(m)

MAX_GRIPPER_OPEN_DISTANCE = 0.056  # Maximum linear distance for fully open gripper (meters)
MAX_GRIPPER_ANGLE = 3.5            # Maximum angle for fully open gripper

joint_positions_left_arm = np.zeros(6)
joint_positions_left_arm_gripper = np.zeros(1)
joint_positions_right_arm = np.zeros(6)
joint_positions_right_arm_gripper = np.zeros(1)

def angle_to_linear_distance(angle):
    return (angle / MAX_GRIPPER_ANGLE) * MAX_GRIPPER_OPEN_DISTANCE

def joint_control_callback(msg):
    global joint_positions_left_arm
    global joint_positions_left_arm_gripper

    joint_positions_left_arm = np.array(msg.joint_pos[:6])
    joint_positions_left_arm_gripper = np.array(msg.joint_pos[6])

def joint_control_callback2(msg2):
    global joint_positions_right_arm
    global joint_positions_right_arm_gripper
    # print("msg2 is {}", msg2.joint_pos)
    joint_positions_right_arm = np.array(msg2.joint_pos[:6])
    joint_positions_right_arm_gripper = np.array(msg2.joint_pos[6])

def move_left_arm():
    d.qpos[:6] = joint_positions_left_arm  

    linear_distance = angle_to_linear_distance(joint_positions_left_arm_gripper)
    d.qpos[6] = linear_distance / 2.0  # left/left_finger
    d.qpos[7] = linear_distance / 2.0  # left/right_finger

def move_right_arm():

    d.qpos[8:14] = joint_positions_right_arm

    linear_distance = angle_to_linear_distance(joint_positions_right_arm_gripper)

    d.qpos[14] = linear_distance / 2.0  # right/left_finger
    d.qpos[15] = linear_distance / 2.0  # right/right_finger

if __name__ == "__main__":
    rospy.init_node('mujoco_joint_controller')

    rospy.Subscriber('/joint_control', JointControl, joint_control_callback)
    rospy.Subscriber('/joint_control2', JointControl, joint_control_callback2)

    d.qpos[:14] = np.zeros(14)

    rate = rospy.Rate(20)
    with viewer.launch_passive(m, d) as v:
        while v.is_running() and not rospy.is_shutdown():

            move_left_arm()

            move_right_arm()

            mujoco.mj_step(m, d)

            v.sync()
            rate.sleep()

    print("Simulation finished.")
```
@Akumar201 Akumar201 added the bug Something isn't working label Oct 29, 2024
@Narsimha-swamy
Copy link

Hi I have the same problem, Here is a video of me trying the simulation, the robots passthrough all the objects from the scene.xml, including the robots and the table. Please, let me know if you find any solution for this. Thanks in advance.

@Akumar201
Copy link
Author

I am still having issue please let me know if you come up with the solution .

@kevinzakka
Copy link
Collaborator

Hi folks, you are setting qpos directly which just teleports the arms but does not exercise the full physics pipeline. You need to actuate the arms by writing to data.ctrl.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

No branches or pull requests

3 participants