From 780b2801814e4836c4a89db2f912006cd1c36715 Mon Sep 17 00:00:00 2001
From: mbennice <mbennice@google.com>
Date: Thu, 23 Aug 2018 13:19:15 -0700
Subject: [PATCH] Allow current positions for Calculate IK.

Allows the user to specify the current positions of joints when
calculating Inverse Kinematics.
---
 examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp | 4 ++++
 examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h   | 4 ++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
index cf148fce5..7f8f29e37 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
@@ -935,6 +935,10 @@ bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct
 		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
 	}
 
+  if (args.m_flags & B3_HAS_CURRENT_POSITIONS) {
+    b3CalculateInverseKinematicsSetCurrentPositions(command, args.m_numDegreeOfFreedom, &args.m_currentJointPositions[0]);
+  }
+
 	b3SharedMemoryStatusHandle statusHandle;
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 
diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
index a58b40d26..3a25b0815 100644
--- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
+++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
@@ -112,13 +112,12 @@ enum b3RobotSimulatorInverseKinematicsFlags
 	B3_HAS_IK_TARGET_ORIENTATION = 1,
 	B3_HAS_NULL_SPACE_VELOCITY = 2,
 	B3_HAS_JOINT_DAMPING = 4,
+  B3_HAS_CURRENT_POSITIONS = 8,
 };
 
 struct b3RobotSimulatorInverseKinematicArgs
 {
 	int m_bodyUniqueId;
-	//	double* m_currentJointPositions;
-	//	int m_numPositions;
 	double m_endEffectorTargetPosition[3];
 	double m_endEffectorTargetOrientation[4];
 	int m_endEffectorLinkIndex;
@@ -129,6 +128,7 @@ struct b3RobotSimulatorInverseKinematicArgs
 	btAlignedObjectArray<double> m_jointRanges;
 	btAlignedObjectArray<double> m_restPoses;
 	btAlignedObjectArray<double> m_jointDamping;
+  btAlignedObjectArray<double> m_currentJointPositions;
 
 	b3RobotSimulatorInverseKinematicArgs()
 		: m_bodyUniqueId(-1),