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

Add inverse kinematics example with implementations by Sam Buss.

Browse Source 
Uses Kuka IIWA model description and 4 methods:
Selectively Damped Least Squares,Damped Least Squares,
Jacobi Transpose, Jacobi Pseudo Inverse
Tweak some PD values in Inverse Dynamics example and Robot example.
This commit is contained in:
erwin coumans
2016-07-24 22:22:42 -07:00
parent 77b9e1a3e2
commit 75e86051c2
29 changed files with 9926 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -23,9 +23,9 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
{
@@ -54,10 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -119,14 +119,14 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
constraintRow.m_orgConstraint = this;