pybullet fast time scale dynamics test

test_pybullet.py

import pybullet as p
import pybullet_data
import time
import numpy as np

SIM_TIMESTEP = 0.0001

# Init sim
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
# Set up the visualizer
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0)
p.setRealTimeSimulation(0)
p.setTimeStep(SIM_TIMESTEP)


p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
planeId = p.loadURDF("plane.urdf")
# load robot
startPos = [0,0,1]
startOrientation = p.getQuaternionFromEuler([0,0,0])
bid = p.loadURDF("urdf/TwoJointRobot.urdf", startPos, startOrientation, useFixedBase=True)

simt = 0
ph = 0
tLastPrint = 0

while simt < 0.5:
	if simt < 0.2:
		freq = 50 # Hz
	elif simt < 0.4:
		freq = 100
	elif simt < 0.6:
		freq = 200
	elif simt < 0.8:
		freq = 400
	else:
		freq = 1000
	# No dynamics: reset positions
	ph = ph + 2 * np.pi * freq * SIM_TIMESTEP
	qdes = 0.5 * np.sin(ph)
	dqdes = 0.5 * 2 * np.pi * freq * np.cos(ph)
	# joint 1 - second link, joint 0 is fixed
	p.setJointMotorControlArray(bid, [1], p.POSITION_CONTROL, targetPositions=[qdes], targetVelocities=[dqdes], positionGains=[10000], velocityGains=[100], forces=[1000000000])

	p.stepSimulation()
	time.sleep(SIM_TIMESTEP)
	simt += SIM_TIMESTEP
	
	if simt - tLastPrint > 0.0005:
		qact = p.getJointState(bid, 1)[0]
		tLastPrint = simt
		print(simt, qdes, qact)

p.disconnect()

TwoJointRobot.urdf

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from TwoJointRobot_woCyl.xacro      | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="TwoJointRobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>
  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
    </inertial>
  </link>
  <link name="link_23_cyl">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
    </inertial>
  </link>
  <link name="link_1">
    <visual>
      <geometry>
        <box size="1.0 0.1 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0.5 0 0"/>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <box size="1.0 0.1 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0.5 0 0"/>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
    </inertial>
  </link>
  <link name="link_2">
    <visual>
      <geometry>
        <box size="1.0 0.1 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0.5 0 0"/>
      <material name="white"/>
    </visual>
    <collision>
      <geometry>
        <box size="1.0 0.1 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0.5 0 0"/>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
    </inertial>
  </link>
  <joint name="joint_1" type="fixed">
    <axis xyz="0 0 1"/>
    <limit effort="1000000" lower="-3.14" upper="3.14" velocity="50000"/>
    <dynamics damping="0" friction="0"/>
    <origin rpy="0 0 0" xyz="0 0 0.075"/>
    <parent link="base_link"/>
    <child link="link_1"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort="1000000" lower="-3.14" upper="3.14" velocity="50000"/>
    <dynamics damping="0" friction="0"/>
    <origin rpy="0 0 0" xyz="1.0 0. 0.05"/>
    <parent link="link_1"/>
    <child link="link_2"/>
  </joint>
  <joint name="link_23" type="fixed">
    <origin rpy="0 0 0" xyz="1.0 0 0"/>
    <parent link="link_2"/>
    <child link="link_23_cyl"/>
  </joint>
</robot>