Friction parameter of tendon

Discussion in 'Simulation' started by reiji, Oct 29, 2017.

  1. Which parameter does decay swing of pendulum?
    I made easy example of pendulum that doesn't take action only swinging in 3D.
    Code:
    <mujoco model="pendulum3d">
        <compiler angle="degree" inertiafromgeom="true"/>
        <default>
            <joint armature="1" damping="1" limited="true"/>
            <geom contype="1" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
        </default>
        <option gravity="0 0 -9.81" integrator="RK4" timestep="0.01"/>
        <visual>
            <global fovy="60" ipd="0.3"/>
        </visual>
        <worldbody>
            <light diffuse=".5 .5 .5" pos="1 1 3" dir="0 0 -1"/>
            <!-- Arena -->
            <geom conaffinity="0" contype="1" name="ground" pos="0 0 0" rgba="0.9 0.9 0.9 1" size="100 100 100" type="plane"/>
            <geom conaffinity="0" fromto="-50. -50. 1.  50.  -50. 1." name="sideS" rgba="0.9 0.4 0.6 1" size=".5" type="capsule"/>
            <geom conaffinity="0" fromto=" 50. -50. 1.  50.   50. 1." name="sideE" rgba="0.9 0.4 0.6 1" size=".5" type="capsule"/>
            <geom conaffinity="0" fromto="-50.  50. 1.  50.   50. 1." name="sideN" rgba="0.9 0.4 0.6 1" size=".5" type="capsule"/>
            <geom conaffinity="0" fromto="-50. -50. 1. -50.   50. 1." name="sideW" rgba="0.9 0.4 0.6 1" size=".5" type="capsule"/>
            <body name="cartapiller" pos="0 0 10">
                <geom type="cylinder" fromto="-1.5 -1 0 1.5 -1 0" size="0.5" friction="0 0 0"/>
                <geom type="cylinder" fromto="-1.5  1 0 1.5  1 0" size="0.5" friction="0 0 0"/>
                <body name="body" pos="0 0 .5">
                    <geom type="box" size="4 2 1" rgba="0 .9 0 1" friction="0 0 0"/>
                    <body name="boom" pos="0 0 1">
                        <geom name="boom" type="capsule" fromto="0 0 0 30 0 20" size="0.5" friction="0 0 0" mass="6000"/>
                        <site name="s4" pos="30 0 20" size="0.5"/>
                        <body name="top" pos="30 0 20">
                            <geom name="top" type="cylinder" fromto="0 -0.5 0 0 0.5 0" size="0.5" friction="0 0 0" mass="500"/>
                        </body>
                    </body>
                </body>
            </body>
    
            <body name="hook" pos="30 0 30">
                <geom name="hook" type="sphere" size="0.005" mass="1"/>
                <site name="s5" pos="0 0 0" size="0.5"/>
                <joint name="hook" type="free" pos="0 0 0" limited="false"/>
            </body>
    
            <body name="tamakake" pos="30 0 28">
                <geom name="tamakake" type="sphere" size="0.005" mass="1"/>
                <site name="s6" pos="0 0 0" size="0.5"/>
                <joint name="tamakake" type="free" pos="0 0 0" limited="false"/>
            </body>
    
            <body name="weight" pos="30 0 26">
                <geom conaffinity="1" contype="1" name="weight" type="box" size="1 1 0.5" mass="8"/>
                <site name="s7" pos="0 0 0" size="0.5"/>
                <joint name="weight" type="free" pos="0 0 0" limited="false"/>
            </body>
    
        </worldbody>
        <tendon>
            <spatial name="top_hook" width="0.2" rgba=".95 .3 .3 1" limited="true" range="1.5 1.5001">
                <site site="s4"/>
                <site site="s5"/>
            </spatial>
            <spatial name="hook_tamakake" width="0.2" rgba=".95 .3 .3 1" limited="true" range="2 2.0001">
                <site site="s5"/>
                <site site="s6"/>
            </spatial>
            <spatial name="tamakake_weight" width="0.2" rgba=".95 .3 .3 1" limited="true" range="2 2.0001">
                <site site="s6"/>
                <site site="s7"/>
            </spatial>
        </tendon>
        <asset>
            <texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0" width="100" height="100"/>
            <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01"
                     rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
            <texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
            <material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
            <material name="geom" texture="texgeom" texuniform="true"/>
        </asset>
    </mujoco>
    
    I ran this asset on gym environment with this code
    Code:
    import numpy as np
    from gym import utils
    from gym.envs.mujoco import mujoco_env
    from gym import spaces
    
    class Pendulum3DEnv(mujoco_env.MujocoEnv, utils.EzPickle):
    
        def __init__(self):
            self.start_done = False
            self.end_done = False
            self.time_step = 0
            utils.EzPickle.__init__(self)
            mujoco_env.MujocoEnv.__init__(self, '/home/rarilurelo/work/deepx_mujoco/assets/pendulum3d.xml', 10)
            high = np.inf*np.ones((1, ))
            low = -high
            self.observation_space = spaces.Box(low, high)
            self.action_space = spaces.Box(0, 1, shape=(1, ))
    
        def _step(self, a):
            for _ in range(self.frame_skip):
                self.model.step()
            self.time_step += 1
            ob = self._get_obs()
            return ob, 10, self.time_step > 500, {}
    
        def _get_obs(self):
            return self.calc_yure()
    
        def viewer_setup(self):
            self.viewer.cam.trackbodyid=0
    
        def calc_yure(self):
            radius_vec = self.get_body_com('top')[:2] - self.get_body_com('cartapiller')[:2]
            weight_vec = self.get_body_com('weight')[:2] - self.get_body_com('top')[:2]
            #print('weight', weight_vec)
            yure = (weight_vec[0] * radius_vec[1] - weight_vec[1] * radius_vec[0]) / np.linalg.norm(radius_vec)
            return yure
    
        def reset_model(self):
            init_yure = 5
            l = 1.5 + 2 + 2
            z3 = l - np.sqrt(l**2 - init_yure**2)
            z2 = (1.5 + 2) / l * z3
            z1 = 1.5 / l * z3
            y3 = init_yure
            y2 = (1.5 + 2) / l * y3
            y1 = 1.5 / l * y3
    
            qpos = np.copy(self.init_qpos)
    
            qpos[1] = qpos[1] + y1
            qpos[2] = qpos[2] + z1
    
            qpos[8] = qpos[8] + y2
            qpos[9] = qpos[9] + z2
    
            qpos[15] = qpos[15] + y3
            qpos[16] = qpos[16] + z3
            qvel = np.copy(self.init_qvel)
            qvel[:] = 0
            self.set_state(qpos, qvel)
            self.time_step = 0
            self.start_done = False
    
            return self._get_obs()
    
        def _render(self, mode='human', close=False):
            if close:
                if self.viewer is not None:
                    self._get_viewer().finish()
                    self.viewer = None
                return
    
            if mode == 'rgb_array':
                self._get_viewer().render()
                data, width, height = self._get_viewer().get_image()
                return np.fromstring(data, dtype='uint8').reshape(height, width, 3)[::-1,:,:]
            elif mode == 'human':
                self._get_viewer().loop_once()
    
    
    
    It decays swing and finally swing becomes 0. However http://www.mujoco.org/book/modeling.html#spatial
    this page says that default friction params are 0.
    which parameter cause this issue?
     
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    You have default joint damping set to 1. This applies automatically to all joints, unless you set damping=0 in the joint definition itself.
     
    reiji likes this.
  3. Thank you!