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

Merge pull request #945 from YunfeiBai/master

Browse Source 
Set joint damping in IK.
This commit is contained in:
erwincoumans
2017-02-03 17:04:25 -08:00
committed by GitHub
parent c1eabcf059 be77fb269f
commit 7e08e960a1
15 changed files with 129 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -148,6 +148,7 @@ public:
m_time+=dt;
m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
m_targetOri.setValue(0, 1.0, 0, 0);
m_targetPos.setValue(0.2*b3Cos( m_time), 0.2*b3Sin( m_time), 1.1);
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
@@ -195,7 +196,8 @@ 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;
ikargs.m_flags |= B3_HAS_JOINT_DAMPING;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
@@ -208,6 +210,7 @@ public:
ikargs.m_upperLimits.resize(numJoints);
ikargs.m_jointRanges.resize(numJoints);
ikargs.m_restPoses.resize(numJoints);
ikargs.m_jointDamping.resize(numJoints,0.5);
ikargs.m_lowerLimits[0] = -2.32;
ikargs.m_lowerLimits[1] = -1.6;
ikargs.m_lowerLimits[2] = -2.32;
@@ -236,6 +239,7 @@ public:
ikargs.m_restPoses[4] = 0;
ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
ikargs.m_restPoses[6] = 0;
ikargs.m_jointDamping[0] = 10.0;
ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))

6
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -516,7 +516,11 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
}
if (args.m_flags & B3_HAS_JOINT_DAMPING)
{
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -86,6 +86,7 @@ enum b3InverseKinematicsFlags
{
B3_HAS_IK_TARGET_ORIENTATION=1,
B3_HAS_NULL_SPACE_VELOCITY=2,
B3_HAS_JOINT_DAMPING=4,
};
struct b3RobotSimInverseKinematicArgs
@@ -102,6 +103,7 @@ struct b3RobotSimInverseKinematicArgs
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3AlignedObjectArray<double> m_jointDamping;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),

17
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -16,6 +16,7 @@ struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
VectorRn m_dampingCoeff;
b3AlignedObjectArray<Node*> m_ikNodes;
@@ -23,6 +24,7 @@ struct IKTrajectoryHelperInternalData
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
};
@@ -136,7 +138,9 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
@@ -201,3 +205,14 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
}
return true;
}
bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff)
{
m_data->m_dampingCoeff.SetLength(numQ);
m_data->m_dampingCoeff.SetZero();
for (int i = 0; i < numQ; ++i)
{
m_data->m_dampingCoeff[i] = coeff[i];
}
return true;
}

1
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -31,6 +31,7 @@ public:
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff);
};
#endif //IK_TRAJECTORY_HELPER_H

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2341,6 +2341,19 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan
}
void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_JOINT_DAMPING;
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_jointDamping[i] = jointDampingCoeff[i];
}
}
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -236,6 +236,7 @@ void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHand
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

22
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4011,12 +4011,30 @@ 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 };
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
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,
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, dampIK);
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -512,7 +512,8 @@ enum EnumCalculateInverseKinematicsFlags
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
IK_HAS_NULL_SPACE_VELOCITY=4,
//IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
IK_HAS_JOINT_DAMPING=8,
//IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
};
struct CalculateInverseKinematicsArgs
@@ -526,6 +527,7 @@ struct CalculateInverseKinematicsArgs
double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM];
double m_restPose[MAX_DEGREE_OF_FREEDOM];
double m_jointDamping[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateInverseKinematicsResultArgs

20
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -389,6 +389,26 @@ void Jacobian::CalcDeltaThetasDLS()
}
}
void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
{
const MatrixRmn& J = ActiveJacobian();
U.SetSize(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::TransposeMultiply(J, J, U);
U.AddToDiagonal( dVec );
dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1);
U.Solve(dT1, &dTheta);
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
dTheta *= MaxAngleDLS/maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithSVD()
{
const MatrixRmn& J = ActiveJacobian();

3
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -68,6 +68,7 @@ public:
void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLS2(const VectorRn& dVec);
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
@@ -136,4 +137,4 @@ public:
};
#endif
#endif

12
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
View File

@@ -251,6 +251,18 @@ MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal en
return *this;
}
// Add a vector to the entries on the diagonal
MatrixRmn& MatrixRmn::AddToDiagonal( const VectorRn& dVec ) // Adds dVec to the diagonal entries
{
long diagLen = Min( NumRows, NumCols );
double* dPtr = x;
for (int i = 0; i < diagLen && i < dVec.GetLength(); ++i) {
*dPtr += dVec[i];
dPtr += NumRows+1;
}
return *this;
}
// Multiply two MatrixRmn's
MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
{

1
examples/ThirdPartyLibs/BussIK/MatrixRmn.h
View File

@@ -110,6 +110,7 @@ public:
// Miscellaneous operation
MatrixRmn& AddToDiagonal( double d ); // Adds d to each diagonal
MatrixRmn& AddToDiagonal( const VectorRn& dVec);
// Solving systems of linear equations
void Solve( const VectorRn& b, VectorRn* x ) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible.

4
examples/pybullet/inverse_kinematics.py
View File

@@ -22,6 +22,8 @@ ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
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])
@@ -64,7 +66,7 @@ while 1:
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)
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)

29
examples/pybullet/pybullet.c
View File

@@ -4430,9 +4430,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* upperLimitsObj = 0;
PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId))
static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj))
{
return NULL;
}
@@ -4452,18 +4453,21 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0;
int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0;
int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0;
int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0;
int numJoints = b3GetNumJoints(sm, bodyIndex);
int hasNullSpace = 0;
int hasJointDamping = 0;
double* lowerLimits = 0;
double* upperLimits = 0;
double* jointRanges = 0;
double* restPoses = 0;
double* jointDamping = 0;
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
(szJointRanges == numJoints) && (szRestPoses == numJoints))
(szJointRanges == numJoints) && (szRestPoses == numJoints))
{
int szInBytes = sizeof(double) * numJoints;
int i;
@@ -4480,11 +4484,23 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
jointRanges[i] =
pybullet_internalGetFloatFromSequence(jointRangesObj, i);
restPoses[i] =
restPoses[i] =
pybullet_internalGetFloatFromSequence(restPosesObj, i);
}
hasNullSpace = 1;
}
if (numJoints && (szJointDamping == numJoints))
{
int szInBytes = sizeof(double) * numJoints;
int i;
jointDamping = (double*)malloc(szInBytes);
for (i = 0; i < numJoints; i++)
{
jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
}
hasJointDamping = 1;
}
if (hasPos)
{
@@ -4514,6 +4530,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos);
}
}
if (hasJointDamping)
{
b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);