diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
index ef3ebf6b8..e432cf438 100644
--- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
+++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
@@ -27,7 +27,9 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
         } else {
             link.joint_type = FLOATING;
         }
-        btTransform transform(btmb->getBaseWorldTransform());
+		btTransform transform=(btmb->getBaseWorldTransform());
+		//compute inverse dynamics in body-fixed frame
+		transform.setIdentity();
 
         link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
         link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 0db35aa69..90fcbea5f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -3638,6 +3638,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
 	command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
 	command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
 	command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+
+	command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
+	command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
+	command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
+	command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
    
 
 }
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 4badd57b2..6341d42f9 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7675,7 +7675,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
 							{
 								const int numDofs = bodyHandle->m_multiBody->getNumDofs();
-
+								int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
                                 b3AlignedObjectArray<double> jacobian_linear;
                                 jacobian_linear.resize(3*numDofs);
                                 b3AlignedObjectArray<double> jacobian_angular;
@@ -7689,11 +7689,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 btAlignedObjectArray<double> q_current;
 								q_current.resize(numDofs);
                                 
-								if (tree && (numDofs == tree->numDoFs()))
+								
+
+								if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
                                 {
                                     jacSize = jacobian_linear.size();
                                     // Set jacobian value
-                                    int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+                                   
                                     
                                     
                                     btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
@@ -7713,8 +7715,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                         -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
                                     {
                                         tree->calculateJacobians(q);
-                                        btInverseDynamics::mat3x jac_t(3, numDofs);
-                                        btInverseDynamics::mat3x jac_r(3,numDofs);
+                                        btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs);
+                                        btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs);
 	                                    // 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);
@@ -7722,8 +7724,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                         {
                                             for (int j = 0; j < numDofs; ++j)
                                             {
-                                                jacobian_linear[i*numDofs+j] = jac_t(i,j);
-                                                jacobian_angular[i*numDofs+j] = jac_r(i,j);
+                                                jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j));
+                                                jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j));
                                             }
                                         }
                                     }
@@ -7789,15 +7791,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
                                
 									btVector3DoubleData endEffectorWorldPosition;
-									btVector3DoubleData endEffectorWorldOrientation;
+									btQuaternionDoubleData endEffectorWorldOrientation;
                                 
-									btVector3 endEffectorPosWorld =  endEffectorTransformWorld.getOrigin();
-									btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
-									btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
+									btVector3 endEffectorPosWorldOrg =  endEffectorTransformWorld.getOrigin();
+									btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation();
+									btTransform endEffectorWorld;
+									endEffectorWorld.setOrigin(endEffectorPosWorldOrg);
+									endEffectorWorld.setRotation(endEffectorOriWorldOrg);
+
+									btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
+
+									btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld;
+
+									btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation();
+
+									btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
                                 
-									endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
-									endEffectorOri.serializeDouble(endEffectorWorldOrientation);
+									endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
+									endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
                                 
+									btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0],
+										clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1],
+										clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
+
+									btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
+										clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
+										clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
+										clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
+									btTransform targetWorld;
+									targetWorld.setOrigin(targetPosWorld);
+									targetWorld.setRotation(targetOrnWorld);
+									btTransform targetBaseCoord;
+									targetBaseCoord = tr.inverse()*targetWorld;
+
+									btVector3DoubleData targetPosBaseCoord;
+									btQuaternionDoubleData targetOrnBaseCoord;
+									targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
+									targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
+
 									// Set joint damping coefficents. A small default
 									// damping constant is added to prevent singularity
 									// with pseudo inverse. The user can set joint damping
@@ -7816,7 +7847,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
                                 
 									double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
-									ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
+									ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
 														   endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
 														   &q_current[0],
 														   numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
diff --git a/examples/pybullet/examples/inverse_dynamics.py b/examples/pybullet/examples/inverse_dynamics.py
index e6af3011b..8b86cc3b9 100644
--- a/examples/pybullet/examples/inverse_dynamics.py
+++ b/examples/pybullet/examples/inverse_dynamics.py
@@ -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):
diff --git a/examples/pybullet/examples/inverse_kinematics_husky_kuka.py b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
new file mode 100644
index 000000000..eb28102b8
--- /dev/null
+++ b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
@@ -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		
diff --git a/examples/pybullet/examples/jointFrictionDamping.py b/examples/pybullet/examples/jointFrictionDamping.py
new file mode 100644
index 000000000..efe34e12f
--- /dev/null
+++ b/examples/pybullet/examples/jointFrictionDamping.py
@@ -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) 
diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py
index 72fa9e27e..55eecd07c 100644
--- a/examples/pybullet/examples/testrender.py
+++ b/examples/pybullet/examples/testrender.py
@@ -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]