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.
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()
Statistics: Posted by joro4o — Fri Jan 05, 2024 7:17 pm
]]>