search

openai / mujoco-py

MuJoCo is a physics engine for detailed, efficient rigid body simulations with contacts. mujoco-py allows using MuJoCo from Python 3.
Other
2.83k stars 810 forks source link

Connecting two bodies with a linkage when both bodies have the same parent. #661

Open mgiglia92 opened 2 years ago

mgiglia92 commented 2 years ago

Hello, I can't seem to find the answer to this between the legacy forum and the new github discussion but hopefully it's an easy one.

I'm modelling a simple 4-wheeled car in Mujoco using the shape primitives available in Mujoco. I took the body_interaction.py example and edited it to make my car. Everything is going well as far as creating the frame, uprights and wheels and orienting them properly. I'm stuck on this next crucial part though.

Typically an ideal steering system is the "ackerman" steering, which is essentially just a 4 bar linkage that controls the orientation of the two front wheels. I'm not sure how I can connect a new "box" type body with two hinge joints on the ends that connect to the bodies named "upright0" and "upright1" such that they rotate together. Note also that the upright0 and upright1 bodies are children of the "robot" body which is just the frame of the vehicle.

Actuating the linkage is another problem, but I think I can figure out how to solve that one once I'm able to constrain the motion of the two bodies by this linkage.

Sorry if this is a repeat question!

Here is the edited body_interaction.py

"""
Example of how bodies interact with each other. For a body to be able to
move it needs to have joints. In this example, the "robot" is a red ball
with X and Y slide joints (and a Z slide joint that isn't controlled).
On the floor, there's a cylinder with X and Y slide joints, so it can
be pushed around with the robot. There's also a box without joints. Since
the box doesn't have joints, it's fixed and can't be pushed around.
"""
from mujoco_py import load_model_from_xml, MjSim, MjViewer
import math
import os
import numpy as np

# Environment parameters
gravity = -9.81

#Model Car Dimensional Parameters (in units m, sec, kg,)
body_x = 0.28
body_y = 0.20
body_z = 0.003
upright_thickness = 0.01
upright_spacing = 0.01
upright_edge_length = 0.04
wheel_diameter = 0.08
wheel_thickness = 0.01
wheel_base = 0.28

# fake coordinate systems
axis_diam = 0.01
axis_length = 0.1
# Helpful pre-written Quaternion values for rotataions

MODEL_XML = f"""
<?xml version="1.0" ?>
<mujoco>
    <option timestep="0.005" gravity="0 0 {gravity}">
        <flag  gravity="enable"/>
    </option>
    <worldbody>
        <body name="xaxis" pos = "1 1 1" quat="1 1 0 0">
            <geom mass="1.0" pos="0 0 0" rgba="1 0 0 0.5" size="0.1 1" type="cylinder"/>
        </body>
        <body name="yaxis" pos = "1 1 1" quat="1 0 1 0">
            <geom mass="1.0" pos="0 0 0" rgba="0 1 0 0.5" size="0.1 1" type="cylinder"/>
        </body>
        <body name="zaxis" pos = "1 1 1" quat = "1 0 0 1">
            <geom mass="1.0" pos="0 0 0" rgba="0 0 1 0.5" size="0.1 1" type="cylinder"/>
        </body>

        <body name="robot" pos="0 0 0.2" quat="1 0 0 0">
            <joint name="slide0" pos="0 0 0" type="free"/>
            <geom mass="10.0" pos="0 0 0" rgba="1 0 0 1" size="{body_x} {body_y} {body_z}" type="box"/>
            <camera euler="0 0 0" fovy="40" name="rgb" pos="0 0 1.5"></camera>

            <body name="upright0" pos = "{body_x} {(body_y) + upright_spacing} 0" quat="1 -1 0 0">
                <geom mass="1.0" pos="0 0 0" rgba="0 1 0 1" size="{upright_edge_length} {upright_edge_length} {upright_thickness}" type="box"/>
                <joint axis="0 1 0" damping="0.0" name="turn0" pos="0 0 0" type="hinge"/>

                <body name="wheel0" pos="0 0 {(wheel_thickness) + upright_thickness}" quat="1 0 0 0">
                    <joint axis="0 0 1" damping="0.0" name="freespin0" pos="0 0 0" type="hinge"/>
                    <geom mass="1.0" pos="0 0 0" rgba="1 0 0 0.5" size="{wheel_diameter} {wheel_thickness}" type="cylinder" solref="0.1"/>
                </body>
            </body>

            <body name="upright1" pos = "{body_x} {-1*(body_y + upright_spacing)} 0" quat="1 1 0 0">
                <geom mass="1.0" pos="0 0 0" rgba="0 1 0 1" size="{upright_edge_length} {upright_edge_length} {upright_thickness}" type="box"/>
                <joint axis="0 1 0" damping="0.0" name="turn1" pos="0 0 0" type="hinge"/>

                <body name="wheel1" pos="0 0 {(wheel_thickness) + upright_thickness}" quat="1 0 0 0">
                    <joint axis="0 0 1" damping="0.0" name="freespin1" pos="0 0 0" type="hinge"/>
                    <geom mass="1.0" pos="0 0 0" rgba="1 0 0 0.5" size="{wheel_diameter} {wheel_thickness}" type="cylinder" solref="0.1"/>
                </body>
            </body>

            <body name="upright2" pos = "{-1*body_x} {(body_y + upright_spacing)} 0" quat="1 -1 0 0">
                <geom mass="1.0" pos="0 0 0" rgba="0 1 0 1" size="{upright_edge_length} {upright_edge_length} {upright_thickness}" type="box"/>
                <body name="wheel2" pos="0 0 {(wheel_thickness) + upright_thickness}" quat="1 0 0 0">
                    <joint axis="0 0 1" damping="0.0" name="power0" pos="0 0 0" type="hinge"/>
                    <geom mass="1.0" pos="0 0 0" rgba="1 0 1 0.5" size="{wheel_diameter} {wheel_thickness}" type="cylinder" friction = "1 1" solref="0.1"/>
                </body>
            </body>

            <body name="upright3" pos = "{-1*body_x} {-1*(body_y + upright_spacing)} 0" quat="1 1 0 0">
                <geom mass="1.0" pos="0 0 0" rgba="0 1 0 1" size="{upright_edge_length} {upright_edge_length} {upright_thickness}" type="box"/>
                <body name="wheel3" pos="0 0 {(wheel_thickness) + upright_thickness}" quat="1 0 0 0">
                    <joint axis="0 0 1" damping="0.0" name="power1" pos="0 0 0" type="hinge"/>
                    <geom mass="1.0" pos="0 0 0" rgba="1 0 1 0.5" size="{wheel_diameter} {wheel_thickness}" type="cylinder" friction = "1 1" solref="0.1"/>
                </body>
            </body>

            <body name="origin" pos="0 0 0">
                <geom mass="1.0" pos="0 0 0" rgba="1 1 1 0.5" size="{axis_diam}" type="sphere"/>
            </body>
            <body name="localx" pos = "0 0 0">
                <geom mass="1.0" pos="{axis_length/2} 0 0" rgba="1 0 0 0.5" size="{axis_diam}" type="sphere"/>
            </body>
            <body name="localy" pos = "0 0 0" >
                <geom mass="1.0" pos="0 {axis_length/2} 0" rgba="0 1 0 0.5" size="{axis_diam}" type="sphere"/>
            </body>
            <body name="localz" pos = "0 0 0">
                <geom mass="1.0" pos="0 0 {axis_length/2}" rgba="0 0 1 0.5" size="{axis_diam}" type="sphere"/>
            </body>
        </body>

        <body mocap="true" name="mocap" pos="0.5 0.5 0.5">
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1.0 1.0 1.0 0.5" size="0.1 0.1 0.1" type="box"></geom>
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1.0 1.0 1.0 0.5" size="0.2 0.2 0.05" type="box"></geom>
        </body>

        <body name="floor" pos="0 0 0.025">
            <geom condim="3" size="10.0 10.0 0.02" rgba="0 1 0 1" type="box"/>
        </body>
    </worldbody>
    <actuator>
        <motor joint="turn0"/>
        <motor joint="power0"/>
        <motor joint="power1"/>
    </actuator>
</mujoco>
"""

model = load_model_from_xml(MODEL_XML)
sim = MjSim(model)
viewer = MjViewer(sim)
t = 0
Ts=0.005
f = 1.0 # Frequency

while True:
    # sim.data.ctrl[0] = math.cos(2*np.pi*t*Ts*f)*0.1
    # sim.data.ctrl[1] = math.sin(2*np.pi*t*Ts)*100
    # sim.data.ctrl[0] = 0.1
    # sim.data.ctrl[1] = -1
    # sim.data.ctrl[2] = -1
    t += 1
    sim.step()
    viewer.render()
    if t > 100 and os.getenv('TESTING') is not None:
        break
mgiglia92 commented 2 years ago

Okay so I finally found the equality/ connect attribute. Playing around with it now.

FrankenApps commented 2 years ago

@mgiglia92 Did you have any luck with the equality/connect attribute? I have a very similar problem (only using a rack and pinion) and I am currently trying to understand which value I need to insert for the anchor. Specifically relative to what coordinate system the anchor has to be (body1, world or body2).

If it has to be in world coordinates it would not help for this specific problem.

In your example it is simple though, because the is no physical connection between the bodies you can simply force the joints to have an opposite value and get the desired effect, by placing this inside the mujoco tag:

<equality>
    <joint joint1="turn0" joint2="turn1" polycoef="0 -1 0 0 0"></joint>
</equality>

However in my case this does not work, because I want to have a physical solid bar in between the steering wheels, that needs to be fixed using joints on both ends (+both ends need to be stationary).

mgiglia92 commented 2 years ago

@mgiglia92 Did you have any luck with the [equality/connect](https://mujoco.readthedocs.io/en/latest

Hi @FrankenApps,

I did end up getting a solution. I simply used the joint option. And like you said I just used the polycoef option and set it to be -1 of the other joint.

<joint joint1="turn0" joint2="turn1" polycoef="0 -1 0 0 0"/>

I did also try using the following, but ended up opting for the simpler option, mostly because I was okay with the non-ackerman steering.

<!-- <connect body1="linkage" body2="upright0" anchor="{body_x - upright_edge_length} {-1*(body_y + upright_spacing)} 0" active="true"/> -->
<!-- <connect body1="linkage" body2="upright1" anchor="{body_x - upright_edge_length} {(body_y + upright_spacing)} 0" active="true"/> -->

As far as positions, I was using the global positions (which I believe are only considered global for when the model is generated and moves properly as the bodies move.) This analysis is probably incorrect though as I am very new to mujoco. I used variables to control the size and positions of the primitives which helped a lot.

Sorry I don't have a great answer to this. What's the reason you can't use global positions to set the anchors?

Just to add to this, I believe the main issue I was having with the equality/connect was the "softness" of the anchor, which was mentioned in another post I made somewhere, I'll try and find it. But this softness comes from the solimp value (and another i can't remember the name of). I think the default values for these make the anchor "soft".