how to generate a uniform motion trajectory

Discussion in 'Simulation' started by Tianhong Dai, Jun 2, 2021.

  1. Tianhong Dai

    Tianhong Dai

    Code:
    from mujoco_py import load_model_from_xml, MjSim, MjViewer
import math
import os
import numpy as np

MODEL_XML = """
<?xml version="1.0" ?>
<mujoco>
    <option timestep="0.005" />
    <worldbody>
        <body name="box2" pos="0.1 0.1 0.4">
            <joint type="free"/>
            <geom mass="1" size="0.05 0.05 0.05" rgba="0.8 0 0 1" type="box" condim="3"/>
        </body>
        <body name="tray" pos="0.1 0.1 0.2">
            <geom name="tray" mass="1" size="0.15 0.15 0.15" type="box" condim="3"/>
            <joint axis="1 0 0" name="cylinder:slidex" type="slide"/>
            <joint axis="0 1 0" name="cylinder:slidey" type="slide"/>
        </body>
        <body name="box" pos="-0.8 0 0.2">
            <geom mass="0.1" size="0.15 0.15 0.15" type="box"/>
        </body>
        <body name="floor" pos="0 0 0.025">
            <geom condim="1" size="5 5 0.02" rgba="0 0.7 0.8 1" type="box"/>
        </body>
    </worldbody>
    <actuator>
        <velocity gear="1.0" joint="cylinder:slidex"/>
        <velocity gear="1.0" joint="cylinder:slidey"/>
    </actuator>
</mujoco>
"""

model = load_model_from_xml(MODEL_XML)
sim = MjSim(model)
viewer = MjViewer(sim)
t = 0
# set intial
vel = sim.data.get_body_xvelp("tray")
vel[0] = np.cos(np.deg2rad(t/2.5))
vel[1] = np.sin(np.deg2rad(t/2.5))
while True:
    t += 1
    sim.step()
    viewer.render()
    sim.data.ctrl[0] = np.cos(np.deg2rad(t/2.5))
    sim.data.ctrl[1] = np.sin(np.deg2rad(t/2.5))
    Hi,

    When I want to generate a trajectory that doing uniform circular motion, I use the following script, however, the model will have an acceleration in the initial and then do the uniform circular motion after few steps. Even after I setting an initial velocity, it still doesn't work. Could I know if there has some way to solve this problem, please?
     
    Tianhong Dai, Jun 2, 2021
    #1