Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
erwincoumans
@@ -143,7 +143,7 @@ if plot:
|
||||
ax_tor.set_ylim(-20., 20.)
|
||||
ax_tor.legend()
|
||||
|
||||
plt.pause(0.01)
|
||||
plt.pause(0.01)
|
||||
|
||||
|
||||
while (1):
|
||||
|
||||
@@ -39,8 +39,9 @@ useNullSpace = 0
|
||||
useOrientation = 1
|
||||
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
|
||||
#This can be used to test the IK result accuracy.
|
||||
useSimulation = 0
|
||||
useSimulation = 1
|
||||
useRealTimeSimulation = 1
|
||||
ikSolver = 0
|
||||
p.setRealTimeSimulation(useRealTimeSimulation)
|
||||
#trailDuration is duration (in seconds) after debug lines will be removed automatically
|
||||
#use 0 for no-removal
|
||||
@@ -68,9 +69,9 @@ while 1:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
|
||||
else:
|
||||
if (useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
|
||||
|
||||
if (useSimulation):
|
||||
for i in range (numJoints):
|
||||
|
||||
169
examples/pybullet/examples/inverse_kinematics_husky_kuka.py
Normal file
169
examples/pybullet/examples/inverse_kinematics_husky_kuka.py
Normal file
@@ -0,0 +1,169 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
from datetime import datetime
|
||||
from datetime import datetime
|
||||
|
||||
clid = p.connect(p.SHARED_MEMORY)
|
||||
if (clid<0):
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("plane.urdf",[0,0,-0.3])
|
||||
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
|
||||
for i in range (p.getNumJoints(husky)):
|
||||
print(p.getJointInfo(husky,i))
|
||||
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
|
||||
ob = kukaId
|
||||
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
|
||||
for jointIndex in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
|
||||
#put kuka on top of husky
|
||||
|
||||
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
|
||||
|
||||
|
||||
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
|
||||
baseorn = [0,0,0,1]
|
||||
#[0, 0, 0.707, 0.707]
|
||||
|
||||
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
|
||||
kukaEndEffectorIndex = 6
|
||||
numJoints = p.getNumJoints(kukaId)
|
||||
if (numJoints!=7):
|
||||
exit()
|
||||
|
||||
|
||||
#lower limits for null space
|
||||
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
|
||||
#upper limits for null space
|
||||
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
|
||||
#joint ranges for null space
|
||||
jr=[5.8,4,5.8,4,5.8,4,6]
|
||||
#restposes for null space
|
||||
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
|
||||
#joint damping coefficents
|
||||
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
|
||||
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(kukaId,i,rp[i])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
t=0.
|
||||
prevPose=[0,0,0]
|
||||
prevPose1=[0,0,0]
|
||||
hasPrevPose = 0
|
||||
useNullSpace = 0
|
||||
|
||||
useOrientation =0
|
||||
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
|
||||
#This can be used to test the IK result accuracy.
|
||||
useSimulation = 0
|
||||
useRealTimeSimulation = 1
|
||||
p.setRealTimeSimulation(useRealTimeSimulation)
|
||||
#trailDuration is duration (in seconds) after debug lines will be removed automatically
|
||||
#use 0 for no-removal
|
||||
trailDuration = 15
|
||||
basepos =[0,0,0]
|
||||
ang = 0
|
||||
ang=0
|
||||
|
||||
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
|
||||
closeEnough = False
|
||||
iter = 0
|
||||
dist2 = 1e30
|
||||
while (not closeEnough and iter<maxIter):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(kukaId,i,jointPoses[i])
|
||||
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
|
||||
newPos = ls[4]
|
||||
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
|
||||
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
|
||||
closeEnough = (dist2 < threshold)
|
||||
iter=iter+1
|
||||
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
|
||||
return jointPoses
|
||||
|
||||
|
||||
wheels=[2,3,4,5]
|
||||
#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
|
||||
#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
|
||||
#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
|
||||
#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
|
||||
wheelVelocities=[0,0,0,0]
|
||||
wheelDeltasTurn=[1,-1,1,-1]
|
||||
wheelDeltasFwd=[1,1,1,1]
|
||||
while 1:
|
||||
keys = p.getKeyboardEvents()
|
||||
shift = 0.01
|
||||
wheelVelocities=[0,0,0,0]
|
||||
speed = 1.0
|
||||
for k in keys:
|
||||
if ord('s') in keys:
|
||||
p.saveWorld("state.py")
|
||||
if ord('a') in keys:
|
||||
basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]]
|
||||
if ord('d') in keys:
|
||||
basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]]
|
||||
|
||||
if p.B3G_LEFT_ARROW in keys:
|
||||
for i in range(len(wheels)):
|
||||
wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i]
|
||||
if p.B3G_RIGHT_ARROW in keys:
|
||||
for i in range(len(wheels)):
|
||||
wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i]
|
||||
if p.B3G_UP_ARROW in keys:
|
||||
for i in range(len(wheels)):
|
||||
wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i]
|
||||
if p.B3G_DOWN_ARROW in keys:
|
||||
for i in range(len(wheels)):
|
||||
wheelVelocities[i] = wheelVelocities[i] -speed*wheelDeltasFwd[i]
|
||||
|
||||
baseorn = p.getQuaternionFromEuler([0,0,ang])
|
||||
for i in range(len(wheels)):
|
||||
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000)
|
||||
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
|
||||
if (useRealTimeSimulation):
|
||||
t = time.time()#(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
|
||||
#t = (dt.second/60.)*2.*math.pi
|
||||
else:
|
||||
t=t+0.001
|
||||
|
||||
if (useSimulation and useRealTimeSimulation==0):
|
||||
p.stepSimulation()
|
||||
|
||||
for i in range (1):
|
||||
#pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
|
||||
pos = [0.2*math.cos(t),0,0.+0.2*math.sin(t)+0.7]
|
||||
#end effector points down, not up (in case useOrientation==1)
|
||||
orn = p.getQuaternionFromEuler([0,-math.pi,0])
|
||||
|
||||
if (useNullSpace==1):
|
||||
if (useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
|
||||
else:
|
||||
if (useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
|
||||
else:
|
||||
threshold =0.001
|
||||
maxIter = 100
|
||||
jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter)
|
||||
|
||||
if (useSimulation):
|
||||
for i in range (numJoints):
|
||||
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=1,velocityGain=0.1)
|
||||
else:
|
||||
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
||||
for i in range (numJoints):
|
||||
p.resetJointState(kukaId,i,jointPoses[i])
|
||||
|
||||
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
|
||||
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
|
||||
59
examples/pybullet/examples/inverse_kinematics_pole.py
Executable file
59
examples/pybullet/examples/inverse_kinematics_pole.py
Executable file
@@ -0,0 +1,59 @@
|
||||
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.01,0.01,0.01,0.01]
|
||||
|
||||
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)
|
||||
|
||||
#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
|
||||
28
examples/pybullet/examples/jointFrictionDamping.py
Normal file
28
examples/pybullet/examples/jointFrictionDamping.py
Normal file
@@ -0,0 +1,28 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("plane.urdf",[0,0,-0.25])
|
||||
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf",useFixedBase=True)
|
||||
print(p.getNumJoints(minitaur))
|
||||
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6,cameraTargetPosition=[-0.064,.621,-0.2])
|
||||
motorJointId = 1
|
||||
p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=100000,force=0)
|
||||
|
||||
p.resetJointState(minitaur,motorJointId,targetValue=0, targetVelocity=1)
|
||||
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0,1,0)
|
||||
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0,0.1,0)
|
||||
|
||||
textId = p.addUserDebugText("jointVelocity=0",[0,0,-0.2])
|
||||
p.setRealTimeSimulation(1)
|
||||
while (1):
|
||||
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
|
||||
angularDamping = p.readUserDebugParameter(angularDampingSlider)
|
||||
p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce)
|
||||
p.changeDynamics(minitaur,motorJointId,linearDamping=0, angularDamping=angularDamping)
|
||||
|
||||
time.sleep(0.01)
|
||||
txt = "jointVelocity="+str(p.getJointState(minitaur,motorJointId)[1])
|
||||
prevTextId = textId
|
||||
textId = p.addUserDebugText(txt,[0,0,-0.2])
|
||||
p.removeUserDebugItem(prevTextId)
|
||||
@@ -3,6 +3,7 @@ import matplotlib.pyplot as plt
|
||||
import pybullet
|
||||
|
||||
pybullet.connect(pybullet.GUI)
|
||||
pybullet.loadURDF("plane.urdf")
|
||||
pybullet.loadURDF("r2d2.urdf")
|
||||
|
||||
camTargetPos = [0.,0.,0.]
|
||||
@@ -18,8 +19,8 @@ pixelWidth = 320
|
||||
pixelHeight = 240
|
||||
nearPlane = 0.01
|
||||
farPlane = 1000
|
||||
lightDirection = [0,1,0]
|
||||
lightColor = [1,1,1]#optional argument
|
||||
lightDirection = [0.4,0.4,0]
|
||||
lightColor = [.3,.3,.3]#1,1,1]#optional argument
|
||||
fov = 60
|
||||
|
||||
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
|
||||
@@ -30,7 +31,7 @@ for pitch in range (0,360,10) :
|
||||
aspect = pixelWidth / pixelHeight;
|
||||
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
|
||||
#getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL
|
||||
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pybullet.ER_TINY_RENDERER)
|
||||
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightAmbientCoeff = 0.3, lightDiffuseCoeff = 0.4, shadow=1, renderer=pybullet.ER_TINY_RENDERER)
|
||||
w=img_arr[0]
|
||||
h=img_arr[1]
|
||||
rgb=img_arr[2]
|
||||
|
||||
@@ -36,7 +36,7 @@ def default():
|
||||
eval_episodes = 25
|
||||
use_gpu = False
|
||||
# Network
|
||||
network = networks.ForwardGaussianPolicy
|
||||
network = networks.feed_forward_gaussian
|
||||
weight_summaries = dict(
|
||||
all=r'.*',
|
||||
policy=r'.*/policy/.*',
|
||||
|
||||
@@ -101,8 +101,6 @@ def train(config, env_processes):
|
||||
"""
|
||||
tf.reset_default_graph()
|
||||
with config.unlocked:
|
||||
config.network = functools.partial(
|
||||
utility.define_network, config.network, config)
|
||||
config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
|
||||
config.value_optimizer = getattr(tf.train, config.value_optimizer)
|
||||
if config.update_every % config.num_agents:
|
||||
|
||||
@@ -98,8 +98,6 @@ def visualize(
|
||||
"""
|
||||
config = utility.load_config(logdir)
|
||||
with config.unlocked:
|
||||
config.network = functools.partial(
|
||||
utility.define_network, config.network, config)
|
||||
config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
|
||||
config.value_optimizer = getattr(tf.train, config.value_optimizer)
|
||||
with tf.device('/cpu:0'):
|
||||
|
||||
@@ -2470,7 +2470,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL};
|
||||
static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
@@ -2487,7 +2487,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
|
||||
|
||||
if (b3GetUserConstraintInfo(sm, constraintUniqueId, &constraintInfo))
|
||||
{
|
||||
PyObject* pyListConstraintInfo = PyTuple_New(11);
|
||||
PyObject* pyListConstraintInfo = PyTuple_New(15);
|
||||
|
||||
PyTuple_SetItem(pyListConstraintInfo, 0, PyLong_FromLong(constraintInfo.m_parentBodyIndex));
|
||||
PyTuple_SetItem(pyListConstraintInfo, 1, PyLong_FromLong(constraintInfo.m_parentJointIndex));
|
||||
@@ -2533,16 +2533,71 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
|
||||
PyTuple_SetItem(pyListConstraintInfo, 9, childFrameOrientation);
|
||||
}
|
||||
PyTuple_SetItem(pyListConstraintInfo, 10, PyFloat_FromDouble(constraintInfo.m_maxAppliedForce));
|
||||
PyTuple_SetItem(pyListConstraintInfo, 11, PyFloat_FromDouble(constraintInfo.m_gearRatio));
|
||||
PyTuple_SetItem(pyListConstraintInfo, 12, PyLong_FromLong(constraintInfo.m_gearAuxLink));
|
||||
PyTuple_SetItem(pyListConstraintInfo, 13, PyFloat_FromDouble(constraintInfo.m_relativePositionTarget));
|
||||
PyTuple_SetItem(pyListConstraintInfo, 14, PyFloat_FromDouble(constraintInfo.m_erp));
|
||||
|
||||
return pyListConstraintInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
PyErr_SetString(SpamError, "Couldn't get user constraint info");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getConstraintState(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
{
|
||||
int constraintUniqueId = -1;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle cmd_handle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
if (b3CanSubmitCommand(sm))
|
||||
{
|
||||
struct b3UserConstraintState constraintState;
|
||||
cmd_handle = b3InitGetUserConstraintStateCommand(sm, constraintUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
if (b3GetStatusUserConstraintState(statusHandle, &constraintState))
|
||||
{
|
||||
if (constraintState.m_numDofs)
|
||||
{
|
||||
PyObject* appliedConstraintForces = PyTuple_New(constraintState.m_numDofs);
|
||||
int i = 0;
|
||||
for (i = 0; i < constraintState.m_numDofs; i++)
|
||||
{
|
||||
PyTuple_SetItem(appliedConstraintForces, i, PyFloat_FromDouble(constraintState.m_appliedConstraintForces[i]));
|
||||
}
|
||||
return appliedConstraintForces;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
PyErr_SetString(SpamError, "Couldn't getConstraintState.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
@@ -7004,7 +7059,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
|
||||
PyObject* targetPosObj = 0;
|
||||
PyObject* targetOrnObj = 0;
|
||||
|
||||
|
||||
int solver = 0; // the default IK solver is DLS
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
PyObject* lowerLimitsObj = 0;
|
||||
@@ -7013,8 +7069,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
PyObject* restPosesObj = 0;
|
||||
PyObject* jointDampingObj = 0;
|
||||
|
||||
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
|
||||
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId))
|
||||
{
|
||||
//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
|
||||
static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
|
||||
@@ -7112,6 +7168,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
int result;
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
|
||||
b3CalculateInverseKinematicsSelectSolver(command, solver);
|
||||
|
||||
if (hasNullSpace)
|
||||
{
|
||||
@@ -7705,6 +7762,9 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get the user-created constraint info, given a constraint unique id."},
|
||||
|
||||
{ "getConstraintState", (PyCFunction)pybullet_getConstraintState, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get the user-created constraint state (applied forces), given a constraint unique id." },
|
||||
|
||||
{"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
|
||||
|
||||
@@ -8139,6 +8199,13 @@ initpybullet(void)
|
||||
|
||||
PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
|
||||
PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
|
||||
|
||||
PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
|
||||
PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);
|
||||
PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION);
|
||||
PyModule_AddIntConstant(m, "IK_HAS_TARGET_ORIENTATION", IK_HAS_TARGET_ORIENTATION);
|
||||
PyModule_AddIntConstant(m, "IK_HAS_NULL_SPACE_VELOCITY", IK_HAS_NULL_SPACE_VELOCITY);
|
||||
PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING);
|
||||
|
||||
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
|
||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
|
||||
|
||||
Reference in New Issue
Block a user