SDF link collisions change robot behavior, despite no interaction with robot

joro4o
Posts: 2
Joined: Tue Oct 25, 2022 8:34 pm

SDF link collisions change robot behavior, despite no interaction with robot

Post by joro4o »

I created a robot that sends motor values to joints at each step of the simulation. I also generate a grid of SDF links. For some reason, the behavior changes when if I drag the SDF links in GUI mode, despite no interaction between them and the robot. Why could this be? I've included relevant code:

generate.py:
--------------------------------------------
import pyrosim.pyrosim as pyrosim
def Create_World():
pyrosim.Start_SDF("world.sdf")

for i in range(10):
for j in range(10):
pyrosim.Send_Cube(name=f"Box{i}", pos=[-10+i*1.5,10+j*1.5,.51] , size=[1,1,1])
pyrosim.End()


def Create_Robot():
pyrosim.Start_URDF("body.urdf")
pyrosim.Send_Cube(name="Torso", pos=[0,0,1.5] , size=[1,1,1])
pyrosim.Send_Joint( name = "Torso_FrontLeg" , parent= "Torso" , child = "FrontLeg" , type = "revolute", position = [0.5,0,1])
pyrosim.Send_Cube(name="FrontLeg", pos=[0.5,0,-0.5] , size=[1,1,1])

pyrosim.Send_Joint( name = "Torso_BackLeg" , parent= "Torso" , child = "BackLeg" , type = "revolute", position = [-0.5,0,1])
pyrosim.Send_Cube(name="BackLeg", pos=[-0.5,0,-0.5] , size=[1,1,1])

pyrosim.End()

Create_World()
Create_Robot()

simulate.py:
----------------------------------
import pybullet_data
import pybullet as p
import time
import pyrosim.pyrosim as pyrosim
import numpy as np
import random

physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())


p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)

p.setGravity(0,0,-9.8)

planeId = p.loadURDF("plane.urdf")
p.loadSDF("world.sdf")
robotId = p.loadURDF("body.urdf")

pyrosim.Prepare_To_Simulate(robotId)

backLegSensorValues = np.zeros(1000)
frontTargetAngles = np.zeros(1000)
backTargetAngles = np.zeros(1000)

frontAmplitude = np.pi/4
frontFrequency = 10
frontPhaseOffset = 0

backAmplitude = np.pi/4
backFrequency = 10
backPhaseOffset = np.pi/3

stateOfLinkZero = p.getBasePositionAndOrientation(robotId)
positionOfLinkZero = stateOfLinkZero[0]
xCoord = positionOfLinkZero[0]
print('Initial X Coordinate= ', xCoord)

for i in range(1000):

p.stepSimulation()
frontTargetAngles = frontAmplitude * np.sin(frontFrequency * i + frontPhaseOffset)
backTargetAngles = backAmplitude * np.sin(backFrequency * i + backPhaseOffset)


pyrosim.Set_Motor_For_Joint(
bodyIndex = robotId,
jointName = "Torso_FrontLeg",
controlMode = p.POSITION_CONTROL,
targetPosition = frontTargetAngles,
maxForce = 500)

pyrosim.Set_Motor_For_Joint(
bodyIndex = robotId,
jointName = "Torso_BackLeg",
controlMode = p.POSITION_CONTROL,
targetPosition = backTargetAngles,
maxForce = 500)

time.sleep(1/4000)


if i%100 == 0 or i == 999:
stateOfLinkZero = p.getBasePositionAndOrientation(robotId)
positionOfLinkZero = stateOfLinkZero[0]
xCoord = positionOfLinkZero[0]
print('X Coordinate= ', xCoord)

p.disconnect()