diff options
| author | yunfeibai <byf1658@gmail.com> | 2016-09-30 00:43:15 -0700 |
|---|---|---|
| committer | yunfeibai <byf1658@gmail.com> | 2016-09-30 00:43:15 -0700 |
| commit | 1c1d3db26dadbb92c4885b77195593bcb6f82937 (patch) | |
| tree | 8d44ee42fe1a59cd7bc582562e28c0af5076e191 | |
| parent | ee00696ae9bc13a6910cd0bffaeb35ff84acbc81 (diff) | |
| download | bullet3-1c1d3db26dadbb92c4885b77195593bcb6f82937.tar.gz | |
An example to compare with and without null space task in IK.
| -rw-r--r-- | examples/RoboticsLearning/KukaGraspExample.cpp | 8 | ||||
| -rw-r--r-- | examples/SharedMemory/IKTrajectoryHelper.cpp | 4 |
2 files changed, 7 insertions, 5 deletions
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index e3c71c738..998e045db 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -195,7 +195,7 @@ public: ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; - ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY; + ikargs.m_flags |= /*B3_HAS_IK_TARGET_ORIENTATION +*/ B3_HAS_NULL_SPACE_VELOCITY; ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; @@ -233,10 +233,10 @@ public: { b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD); t.m_targetPosition = ikresults.m_calculatedJointPositions[i]; - t.m_maxTorqueValue = 100; - t.m_kp= 1; + t.m_maxTorqueValue = 100.0; + t.m_kp= 1.0; t.m_targetVelocity = 0; - t.m_kp = 0.5; + t.m_kd = 1.0; m_robotSim.setJointMotorControl(m_kukaIndex,i,t); } diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index f671c5951..7c9793c7f 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -181,10 +181,12 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, double stayCloseToZeroGain = 0.1; double stayAwayFromLimitsGain = 10.0; + double q_rest[7] = {0, 0, 0, SIMD_HALF_PI, 0, -SIMD_HALF_PI*0.66, 0}; + // Stay close to zero for (int i = 0; i < numQ; ++i) { - m_data->m_nullSpaceVelocity[i] = -stayCloseToZeroGain * q_current[i]; + m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (q_rest[i]-q_current[i]); } // Stay away from joint limits |