forked from erwincoumans/pybullet_robots
-
Notifications
You must be signed in to change notification settings - Fork 0
/
dobot.py
56 lines (43 loc) · 2.23 KB
/
dobot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#Created and copyright by Erwin Coumans for http://pybullet.org
import pybullet as p
p.connect(p.GUI)
#p.loadURDF("plane_transparent2.urdf")
p.loadURDF("plane.urdf")
dobot = p.loadURDF("dobot/dobot.urdf",useFixedBase=True)
p.setRealTimeSimulation(1)
p.setPhysicsEngineParameter(numSolverIterations=300)
p.setPhysicsEngineParameter(numSubSteps=10)
for i in range (p.getNumJoints(dobot)):
print(p.getJointInfo(dobot,i))
p.setJointMotorControl2(dobot,i,p.VELOCITY_CONTROL,targetVelocity=0,force=20)
#p.setJointMotorControl2(dobot,1,p.VELOCITY_CONTROL,targetVelocity=0.1,force=100)
#p.setJointMotorControl2(dobot,2,p.VELOCITY_CONTROL,targetVelocity=0.,force=100)
p.setJointMotorControl2(dobot,0,p.VELOCITY_CONTROL,targetVelocity=0,force=1000)
p.setJointMotorControl2(dobot,3,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,5,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,6,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,8,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.resetJointState(dobot,6,-1.57)
p.resetJointState(dobot,7,-1.57)
p.resetJointState(dobot,8,1.57)
#colors for debugging
#p.changeVisualShape(dobot,4,rgbaColor=[1,1,1,1])
#p.changeVisualShape(dobot,3,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,6,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,5,rgbaColor=[1,1,0,1])
c = p.createConstraint(dobot,6,dobot,8,jointType=p.JOINT_POINT2POINT,jointAxis =[1,0,0],parentFramePosition=[-0.05,0,0.08],childFramePosition=[-0.05,0.026,-0.006])#0.014-0.02 (inertial com)
c = p.createConstraint(dobot,3,dobot,5,jointType=p.JOINT_POINT2POINT,jointAxis =[1,0,0],parentFramePosition=[-0.05,0,0.075],childFramePosition=[-0.05,-0.026,0.014])
0.150
p.getCameraImage(320,200)#160,100)
p.resetDebugVisualizerCamera(1,85.6,0,[-0.61,0.12,0.25])
p.setJointMotorControl2(dobot,6,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
for i in range (p.getNumJoints(dobot)):
worldLinkFramePosition = p.getLinkState(dobot,i)[4]
worldLinkFrameOrientation = p.getLinkState(dobot,i)[5]
print(i,worldLinkFramePosition)
while (1):
p.setGravity(0,0,0)
ls = p.getLinkState(dobot,5)
linkOrn = ls[1]
eul = p.getEulerFromQuaternion(linkOrn)
#print(eul)