Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Modify the link index when computing Jacobian. Add a test for end effector orientation IK. Inverse dynamics Jacobian uses zero-based indexing of bodies, not starting from -1 for base.

Browse Source
This commit is contained in:
yunfeibai
2017-04-17 16:54:45 -07:00
parent ee753eef6f
commit a7068bb57a
2 changed files with 36 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4639,7 +4639,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
// Set jacobian value
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex, &jac_t);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
@@ -5065,8 +5065,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
@@ -6235,8 +6236,8 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3,numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)

30
examples/pybullet/examples/ik_end_effector_orientation.py Normal file
View File

@@ -0,0 +1,30 @@
import pybullet as p
import time
import math
from datetime import datetime
from time import sleep
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd=[100.0,100.0,100.0,100.0,100.0,100.0,0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0,0,0)
while 1:
p.stepSimulation()
for i in range (1):
pos = [0,0,1.26]
orn = p.getQuaternionFromEuler([0,0,3.14])
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
for i in range (numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
sleep(0.05)