From 3910003e0a57fdb7852b851a55366cbef04dd28d Mon Sep 17 00:00:00 2001 From: YunfeiBai Date: Tue, 27 Sep 2016 16:37:49 -0700 Subject: [PATCH] Add IK damping coefficient vector. --- ... => wsg50_one_motor_gripper_free_base.sdf} | 0 data/table.urdf | 61 ------------- data/tray.urdf | 91 ------------------- examples/SharedMemory/IKTrajectoryHelper.cpp | 9 +- examples/SharedMemory/IKTrajectoryHelper.h | 13 ++- .../PhysicsServerCommandProcessor.cpp | 19 +--- 6 files changed, 15 insertions(+), 178 deletions(-) rename data/gripper/{wsg50_one_motor_gripper_new.sdf => wsg50_one_motor_gripper_free_base.sdf} (100%) delete mode 100644 data/table.urdf delete mode 100644 data/tray.urdf diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf similarity index 100% rename from data/gripper/wsg50_one_motor_gripper_new.sdf rename to data/gripper/wsg50_one_motor_gripper_free_base.sdf diff --git a/data/table.urdf b/data/table.urdf deleted file mode 100644 index 07d819315..000000000 --- a/data/table.urdf +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/data/tray.urdf b/data/tray.urdf deleted file mode 100644 index 32325a815..000000000 --- a/data/tray.urdf +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index ff93d7bbb..c4ee51b0e 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -43,7 +43,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], const double endEffectorWorldPosition[3], const double endEffectorWorldOrientation[4], const double* q_current, int numQ,int endEffectorIndex, - double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk) + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]) { bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false; @@ -69,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], VectorRn deltaS(3); for (int i = 0; i < 3; ++i) { - deltaS.Set(i,dampIk*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])); + deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])); } // Set one end effector world orientation from Bullet @@ -79,13 +79,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], btQuaternion deltaQ = endQ*startQ.inverse(); float angle = deltaQ.getAngle(); btVector3 axis = deltaQ.getAxis(); - float angleDot = angle*dampIk; + float angleDot = angle; btVector3 angularVel = angleDot*axis.normalize(); for (int i = 0; i < 3; ++i) { - deltaR.Set(i,angularVel[i]); + deltaR.Set(i,dampIk[i+3]*angularVel[i]); } - deltaR[2] = 0.0; { diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index b4783b41d..c8bb761c2 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -21,13 +21,12 @@ public: IKTrajectoryHelper(); virtual ~IKTrajectoryHelper(); - - bool computeIK(const double endEffectorTargetPosition[3], - const double endEffectorTargetOrientation[4], - const double endEffectorWorldPosition[3], - const double endEffectorWorldOrientation[4], - const double* q_old, int numQ,int endEffectorIndex, - double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk=1.); + bool computeIK(const double endEffectorTargetPosition[3], + const double endEffectorTargetOrientation[4], + const double endEffectorWorldPosition[3], + const double endEffectorWorldOrientation[4], + const double* q_old, int numQ, int endEffectorIndex, + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 071b0f338..7e3a057c7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2650,12 +2650,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorOri.serializeDouble(endEffectorWorldOrientation); - + double dampIK[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, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, &q_current[0], numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2); + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK); serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; for (int i=0;im_KukaId = bodyId; // Load one motor gripper for kuka - loadSdf("gripper/wsg50_one_motor_gripper_new.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadSdf("gripper/wsg50_one_motor_gripper_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); m_data->m_gripperId = bodyId + 1; InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); @@ -3013,15 +3013,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - // Table area - loadUrdf("table.urdf", btVector3(0, -1.9, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("tray.urdf", btVector3(0, -1.7, 0.64), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cup_small.urdf", btVector3(0.5, -2.0, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("pitcher_small.urdf", btVector3(0.4, -1.8, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, -1.9, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", btVector3(0.1, -1.8, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere_small.urdf", btVector3(-0.2, -1.7, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; @@ -3105,7 +3096,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) if (closeToKuka) { - int dampIk = 1; + double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0}; IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); IKTrajectoryHelper* ikHelperPtr = 0;