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

fix minitaur.urdf: move lower-leg inertia to center, add missing collision for one of the motors, add contact parameters for friction_anchor, spinning friction, compliance (stiffness/damping)

Browse Source 
fix in indexing for maximal coordinates (unused by default, still experimental, requires many iterations for Minitaur due to extreme mass-ratio, hence use of reduces/generalized coordinates)
modify quadruped.py to test maximal coordinates
wrap angular servo (positional) target within [-PI,PI] in btGeneric6DofSpring2Constraint
add 'j' key to show body frames in wireframe/debug mode
This commit is contained in:
erwincoumans
2017-09-02 01:05:42 -07:00
parent ee30479a28
commit eb97e06280
9 changed files with 281 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
data/quadruped/minitaur.urdf
View File

@@ -267,6 +267,11 @@
<color rgba="1 1 1 1"/> <color rgba="1 1 1 1"/>
</material> </material>
</visual> </visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial> <inertial>
<mass value="0.25"/> <mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
@@ -427,8 +432,10 @@
<link name="lower_leg_front_rightR_link"> <link name="lower_leg_front_rightR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -448,11 +455,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_rightR_link" type="revolute"> <joint name="knee_front_rightR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/> <parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/> <child link="lower_leg_front_rightR_link"/>
@@ -495,8 +503,10 @@
<link name="lower_leg_front_rightL_link"> <link name="lower_leg_front_rightL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -516,11 +526,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_rightL_link" type="revolute"> <joint name="knee_front_rightL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/> <parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/> <child link="lower_leg_front_rightL_link"/>
@@ -561,8 +572,10 @@
<link name="lower_leg_front_leftR_link"> <link name="lower_leg_front_leftR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -582,11 +595,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_leftR_link" type="revolute"> <joint name="knee_front_leftR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/> <parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/> <child link="lower_leg_front_leftR_link"/>
@@ -628,8 +642,10 @@
<link name="lower_leg_front_leftL_link"> <link name="lower_leg_front_leftL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -649,11 +665,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_leftL_link" type="revolute"> <joint name="knee_front_leftL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/> <parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/> <child link="lower_leg_front_leftL_link"/>
@@ -692,8 +709,10 @@
<link name="lower_leg_back_rightR_link"> <link name="lower_leg_back_rightR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -713,11 +732,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_rightR_link" type="revolute"> <joint name="knee_back_rightR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/> <parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/> <child link="lower_leg_back_rightR_link"/>
@@ -760,8 +780,10 @@
<link name="lower_leg_back_rightL_link"> <link name="lower_leg_back_rightL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -781,11 +803,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_rightL_link" type="revolute"> <joint name="knee_back_rightL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/> <parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/> <child link="lower_leg_back_rightL_link"/>
@@ -826,8 +849,10 @@
<link name="lower_leg_back_leftR_link"> <link name="lower_leg_back_leftR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -847,11 +872,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_leftR_link" type="revolute"> <joint name="knee_back_leftR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/> <parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/> <child link="lower_leg_back_leftR_link"/>
@@ -891,8 +917,10 @@
<link name="lower_leg_back_leftL_link"> <link name="lower_leg_back_leftL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -912,11 +940,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_leftL_link" type="revolute"> <joint name="knee_back_leftL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/> <parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/> <child link="lower_leg_back_leftL_link"/>

5
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -221,6 +221,11 @@ void MyKeyboardCallback(int key, int state)
gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation; gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0); gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
} }
if (key == 'j' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawFrames;
}
if (key == 'k' && state) if (key == 'k' && state)
{ {
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints; gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -140,7 +140,7 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
dof6->setLinearUpperLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1)); dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0)); dof6->setAngularLowerLimit(btVector3(0,0,1));
} }
}; };

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -396,7 +396,7 @@ template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex,
info.m_linkName = 0; info.m_linkName = 0;
info.m_flags = 0; info.m_flags = 0;
info.m_jointIndex = linkIndex; info.m_jointIndex = linkIndex;
info.m_qIndex = linkIndex+6; info.m_qIndex = linkIndex+7;
info.m_uIndex = linkIndex+6; info.m_uIndex = linkIndex+6;
//derive type from limits //derive type from limits

51
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4601,7 +4601,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Using CONTROL_MODE_TORQUE"); b3Printf("Using CONTROL_MODE_TORQUE");
} }
// mb->clearForcesAndTorques(); // mb->clearForcesAndTorques();
int torqueIndex = 6; ///see addJointInfoFromConstraint
int velIndex = 6;
int posIndex = 7;
//if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{ {
for (int link=0;link<body->m_rigidBodyJoints.size();link++) for (int link=0;link<body->m_rigidBodyJoints.size();link++)
@@ -4616,14 +4618,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++) //for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{ {
double torque = 0.f;
//if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
btScalar qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[torqueIndex];
btScalar qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[torqueIndex];
{
int torqueIndex = velIndex;
double torque = 100;
bool hasDesiredTorque = false;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
hasDesiredTorque = true;
}
bool hasDesiredPosOrVel = false;
btScalar qdotTarget = 0.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
{
hasDesiredPosOrVel = true;
qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
}
btScalar qTarget = 0.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
{
hasDesiredPosOrVel = true;
qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
}
con->getLinearLowerLimit(linearLowerLimit); con->getLinearLowerLimit(linearLowerLimit);
con->getLinearUpperLimit(linearUpperLimit); con->getLinearUpperLimit(linearUpperLimit);
con->getAngularLowerLimit(angularLowerLimit); con->getAngularLowerLimit(angularLowerLimit);
@@ -4649,24 +4670,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{ {
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{
if (hasDesiredTorque)
{ {
con->getRigidBodyA().applyTorque(torque*axisA); con->getRigidBodyA().applyTorque(torque*axisA);
con->getRigidBodyB().applyTorque(-torque*axisB); con->getRigidBodyB().applyTorque(-torque*axisB);
}
break; break;
} }
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{
if (hasDesiredPosOrVel)
{ {
con->enableMotor(3+limitAxis,true); con->enableMotor(3+limitAxis,true);
con->setTargetVelocity(3+limitAxis, qdotTarget); con->setTargetVelocity(3+limitAxis, qdotTarget);
//this is max motor force impulse //this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse); con->setMaxMotorForce(3+limitAxis,torqueImpulse);
}
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{
if (hasDesiredPosOrVel)
{ {
con->setServo(3+limitAxis,true); con->setServo(3+limitAxis,true);
con->setServoTarget(3+limitAxis,qTarget); con->setServoTarget(3+limitAxis,-qTarget);
//next one is the maximum velocity to reach target position. //next one is the maximum velocity to reach target position.
//the maximum velocity is limited by maxMotorForce //the maximum velocity is limited by maxMotorForce
con->setTargetVelocity(3+limitAxis, 100); con->setTargetVelocity(3+limitAxis, 100);
@@ -4674,6 +4703,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse); con->setMaxMotorForce(3+limitAxis,torqueImpulse);
con->enableMotor(3+limitAxis,true); con->enableMotor(3+limitAxis,true);
}
break; break;
} }
default: default:
@@ -4733,7 +4763,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
}//fi }//fi
torqueIndex++; ///see addJointInfoFromConstraint
velIndex ++;//info.m_uIndex
posIndex ++;//info.m_qIndex
} }
} }
}//fi }//fi

123
examples/pybullet/examples/quadruped.py
View File

@@ -2,34 +2,49 @@ import pybullet as p
import time import time
import math import math
useRealTime = 0 toeConstraint = True
fixedTimeStep = 0.01 useMaximalCoordinates = True
useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1./100
numSolverIterations = 50
if (useMaximalCoordinates):
fixedTimeStep = 1./500
numSolverIterations = 200
speed = 10 speed = 10
amplitude = 0.8 amplitude = 0.8
jump_amp = 0.5 jump_amp = 0.5
maxForce = 3.5 maxForce = 3.5
kneeFrictionForce = 0.00 kneeFrictionForce = 0
kp = 1 kp = 1
kd = .1 kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId<0): if (physId<0):
p.connect(p.GUI) p.connect(p.GUI)
#p.resetSimulation() #p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50) angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0,angle,0])
p.loadURDF("plane.urdf",[0,0,0],orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4) p.setTimeOut(4000000)
p.setGravity(0,0,0) p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep) p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4]) orn = p.getQuaternionFromEuler([0,0,0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False) quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
for i in range(nJoints): for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i) jointInfo = p.getJointInfo(quadruped, i)
@@ -37,11 +52,11 @@ for i in range(nJoints):
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
@@ -61,49 +76,81 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir=[-1,-1,-1,-1,1,1,1,1] motordir=[-1,-1,-1,-1,1,1,1,1]
halfpi = 1.57079632679 halfpi = 1.57079632679
twopi = 4*halfpi
kneeangle = -2.1834 kneeangle = -2.1834
if (useMaximalCoordinates):
steps = 400
for aa in range (steps):
p.setJointMotorControl2(quadruped,motor_front_leftL_joint,p.POSITION_CONTROL,motordir[0]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightL_joint,p.POSITION_CONTROL,motordir[6]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftL_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightR_link,p.POSITION_CONTROL,motordir[7]*kneeangle*float(aa)/steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
#p.getNumJoints(1)
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
if (1):
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
@@ -135,10 +182,18 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMo
#stand still #stand still
p.setRealTimeSimulation(useRealTime) p.setRealTimeSimulation(useRealTime)
#while (True):
# time.sleep(0.01)
#p.stepSimulation()
t = 0.0
t_end = t + 15
ref_time = time.time()
while (t<t_end):
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
if (useRealTime==0):
p.stepSimulation()
time.sleep(fixedTimeStep)
print("quadruped Id = ") print("quadruped Id = ")
print(quadruped) print(quadruped)
@@ -156,7 +211,7 @@ t_end = t + 100
i=0 i=0
ref_time = time.time() ref_time = time.time()
for aa in range (100): while (1):
if (useRealTime): if (useRealTime):
t = time.time()-ref_time t = time.time()-ref_time
else: else:

2
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1333,7 +1333,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
// Draw a small simplex at the center of the object // Draw a small simplex at the center of the object
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
{ {
getDebugDrawer()->drawTransform(worldTransform,1); getDebugDrawer()->drawTransform(worldTransform,.1);
} }
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)

63
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -729,6 +729,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (limot->m_enableMotor && limot->m_servoMotor) if (limot->m_enableMotor && limot->m_servoMotor)
{ {
btScalar error = limot->m_currentPosition - limot->m_servoTarget; btScalar error = limot->m_currentPosition - limot->m_servoTarget;
btScalar curServoTarget = limot->m_servoTarget;
if (rotational)
{
if (error > SIMD_PI)
{
error -= SIMD_2_PI;
curServoTarget +=SIMD_2_PI;
}
if (error < -SIMD_PI)
{
error += SIMD_2_PI;
curServoTarget -=SIMD_2_PI;
}
}
calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
btScalar tag_vel = -targetvelocity; btScalar tag_vel = -targetvelocity;
@@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar hiLimit; btScalar hiLimit;
if(limot->m_loLimit > limot->m_hiLimit) if(limot->m_loLimit > limot->m_hiLimit)
{ {
lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
} }
else else
{ {
lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; hiLimit = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
} }
mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
} }
@@ -998,14 +1013,50 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc
m_angularLimits[index - 3].m_targetVelocity = velocity; m_angularLimits[index - 3].m_targetVelocity = velocity;
} }
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
{ {
btAssert((index >= 0) && (index < 6)); btAssert((index >= 0) && (index < 6));
if (index<3) if (index<3)
m_linearLimits.m_servoTarget[index] = target; {
m_linearLimits.m_servoTarget[index] = targetOrg;
}
else else
{
//wrap between -PI and PI, see also
//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
btScalar target = targetOrg+SIMD_PI;
if (1)
{
btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
// handle boundary cases resulted from floating-point cut off:
{
if (m>=SIMD_2_PI)
{
target = 0;
} else
{
if (m<0 )
{
if (SIMD_2_PI+m == SIMD_2_PI)
target = 0;
else
target = SIMD_2_PI+m;
}
else
{
target = m;
}
}
}
target -= SIMD_PI;
}
m_angularLimits[index - 3].m_servoTarget = target; m_angularLimits[index - 3].m_servoTarget = target;
} }
}
void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
{ {

2
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h" #include "LinearMath/btSerializer.h"
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type), :btTypedObject(type),