64 lines
2.0 KiB
Python
Executable File
64 lines
2.0 KiB
Python
Executable File
import pybullet as p
|
|
import time
|
|
import math
|
|
from datetime import datetime
|
|
|
|
clid = p.connect(p.SHARED_MEMORY)
|
|
if (clid < 0):
|
|
p.connect(p.GUI)
|
|
p.loadURDF("plane.urdf", [0, 0, -1.3])
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
|
sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
|
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
|
|
|
|
sawyerEndEffectorIndex = 3
|
|
numJoints = p.getNumJoints(sawyerId)
|
|
#joint damping coefficents
|
|
jd = [0.1, 0.1, 0.1, 0.1]
|
|
|
|
p.setGravity(0, 0, 0)
|
|
t = 0.
|
|
prevPose = [0, 0, 0]
|
|
prevPose1 = [0, 0, 0]
|
|
hasPrevPose = 0
|
|
|
|
ikSolver = 0
|
|
useRealTimeSimulation = 0
|
|
p.setRealTimeSimulation(useRealTimeSimulation)
|
|
#trailDuration is duration (in seconds) after debug lines will be removed automatically
|
|
#use 0 for no-removal
|
|
trailDuration = 1
|
|
|
|
while 1:
|
|
if (useRealTimeSimulation):
|
|
dt = datetime.now()
|
|
t = (dt.second / 60.) * 2. * math.pi
|
|
else:
|
|
t = t + 0.01
|
|
time.sleep(0.01)
|
|
|
|
for i in range(1):
|
|
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
|
|
jointPoses = p.calculateInverseKinematics(sawyerId,
|
|
sawyerEndEffectorIndex,
|
|
pos,
|
|
jointDamping=jd,
|
|
solver=ikSolver,
|
|
maxNumIterations=100)
|
|
|
|
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
|
for i in range(numJoints):
|
|
jointInfo = p.getJointInfo(sawyerId, i)
|
|
qIndex = jointInfo[3]
|
|
if qIndex > -1:
|
|
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
|
|
|
|
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
|
|
if (hasPrevPose):
|
|
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
|
|
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
|
|
prevPose = pos
|
|
prevPose1 = ls[4]
|
|
hasPrevPose = 1
|