Added manually converted widowx.urdf from https://github.com/RobotnikAutomation/widowx_arm

Added simple pybullet file in Bullet/examples/pybullet/examples/widows.py (preliminary, both URDF and py file needs more work to be useful)
https://github.com/RobotnikAutomation/widowx_arm/blob/master/widowx_arm_description/package.xml
See also http://www.trossenrobotics.com/widowxrobotarm
This commit is contained in:
Erwin Coumans
2017-06-29 17:54:04 -07:00
parent b4f21e77af
commit bf800e30d5
11 changed files with 322 additions and 0 deletions

View File

@@ -0,0 +1,21 @@
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True)
p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000])
while (1):
p.stepSimulation()
time.sleep(0.01)
#p.saveWorld("test.py")
viewMat = p.getDebugVisualizerCamera()[2]
#projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = p.getDebugVisualizerCamera()[3]
width=640
height=480
img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)