AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1393 from YunfeiBai/master

Browse Source 
Expose IK solver options: DLS and SDLS.
This commit is contained in:
erwincoumans 2017-10-20 13:22:56 -07:00 committed by GitHub
commit 28ea41bfe6
12 changed files with 263 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
data/pole.urdf Executable file
View File

@ -0,0 +1,140 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="slideBar">
<visual>
<geometry>
<box size="30 0.05 0.05"/>
</geometry>
<origin xyz="0 0 0"/>
<material name="green">
<color rgba="0 0.8 .8 1"/>
</material>
</visual>
<inertial>
<mass value="0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="pole">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="cart_to_pole" type="prismatic">
<axis xyz="1 0 0"/>
<origin xyz="0.0 0.0 0.5"/>
<parent link="slideBar"/>
<child link="pole"/>
<limit effort="1000.0" lower="-5" upper="5" velocity="0.5"/>
</joint>
<link name="pole2">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="pole_to_pole2" type="continuous">
<axis xyz="1 0 0"/>
<origin xyz="0.0 0.0 0.5"/>
<parent link="pole"/>
<child link="pole2"/>
</joint>
<link name="pole3">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="pole2_to_pole3" type="continuous">
<axis xyz="0 1 0"/>
<origin xyz="0.0 0.0 1"/>
<parent link="pole2"/>
<child link="pole3"/>
</joint>
<link name="endeffector">
<visual>
<geometry>
<box size="0.06 0.06 .06"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.06 0.06 .06"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="pole2_to_endeffector" type="fixed">
<origin xyz="0.0 0.0 1"/>
<parent link="pole3"/>
<child link="endeffector"/>
</joint>
</robot>

7
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@ -47,7 +47,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,int endEffectorIndex,
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 || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
|| ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:

2
examples/SharedMemory/IKTrajectoryHelper.h
View File

@ -12,6 +12,8 @@ enum IK2_Method
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
IK2_VEL_SDLS,
IK2_VEL_SDLS_WITH_ORIENTATION,
};

8
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -3721,6 +3721,14 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCom
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= solver;
}
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -338,6 +338,7 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API 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);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

18
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -7731,19 +7731,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
}
else
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS;
}
else
{
ikMethod = IK2_VEL_DLS;;
}
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)

9
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -633,15 +633,6 @@ struct CalculateMassMatrixResultArgs
int m_dofCount;
};
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
IK_HAS_NULL_SPACE_VELOCITY=4,
IK_HAS_JOINT_DAMPING=8,
//IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -572,6 +572,18 @@ enum EnumRenderer
//ER_FIRE_RAYS=(1<<18),
};
///flags to pick the IK solver and other options
enum EnumCalculateInverseKinematicsFlags
{
IK_DLS=0,
IK_SDLS=1, //TODO: can add other IK solvers
IK_HAS_TARGET_POSITION=16,
IK_HAS_TARGET_ORIENTATION=32,
IK_HAS_NULL_SPACE_VELOCITY=64,
IK_HAS_JOINT_DAMPING=128,
//IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
};
enum b3ConfigureDebugVisualizerEnum
{
COV_ENABLE_GUI=1,

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

@ -462,7 +462,8 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
int nRows = J.GetNumRows();
int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three
// TODO: Modify it to work with multiple end effectors.
int numEndEffectors = 1;
int nCols = J.GetNumColumns();
dTheta.SetZero();

7
examples/pybullet/examples/inverse_kinematics.py
View File

@ -39,8 +39,9 @@ useNullSpace = 0
useOrientation = 1
#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
useSimulation = 1
useRealTimeSimulation = 1
ikSolver = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
@ -68,9 +69,9 @@ 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,jointDamping=jd)
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver)
else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
if (useSimulation):
for i in range (numJoints):

59
examples/pybullet/examples/inverse_kinematics_pole.py Executable file
View File

@ -0,0 +1,59 @@
import pybullet as p
import time
import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
sawyerId = p.loadURDF("pole.urdf",[0,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd=[0.01,0.01,0.01,0.01]
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
hasPrevPose = 0
ikSolver = 0
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 1
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+0.01
time.sleep(0.01)
for i in range (1):
pos = [2.*math.cos(t),2.*math.cos(t),0.+2.*math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId,i,jointPoses[qIndex-7])
ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex)
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

13
examples/pybullet/pybullet.c
View File

@ -7050,6 +7050,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* targetPosObj = 0;
PyObject* targetOrnObj = 0;
int solver = 0; // the default IK solver is DLS
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
PyObject* lowerLimitsObj = 0;
@ -7058,8 +7059,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId))
{
//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
@ -7157,6 +7158,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
b3CalculateInverseKinematicsSelectSolver(command, solver);
if (hasNullSpace)
{
@ -8188,6 +8190,13 @@ initpybullet(void)
PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);
PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION);
PyModule_AddIntConstant(m, "IK_HAS_TARGET_ORIENTATION", IK_HAS_TARGET_ORIENTATION);
PyModule_AddIntConstant(m, "IK_HAS_NULL_SPACE_VELOCITY", IK_HAS_NULL_SPACE_VELOCITY);
PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING);
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);