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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2016-09-01 21:26:38 -07:00
parent e47c74ce0b e98fca1e5e
commit fe54f146ad
47 changed files with 1759 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
data/cube_gripper_left.urdf Normal file
View File

@@ -0,0 +1,45 @@
<?xml version="0.0" ?>
<robot name="cube_gripper_left.urdf">
<link name="world">
</link>
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale=".05 .05 .05"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
</collision>
</link>
<joint name="cube_joint" type="prismatic">
<parent link="world"/>
<child link="baseLink"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.0"/>
</joint>
</robot>

45
data/cube_gripper_right.urdf Normal file
View File

@@ -0,0 +1,45 @@
<?xml version="0.0" ?>
<robot name="cube_gripper_right.urdf">
<link name="world">
</link>
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale=".05 .05 .05"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
</collision>
</link>
<joint name="cube_joint" type="prismatic">
<parent link="world"/>
<child link="baseLink"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
<dynamics damping="0.0"/>
</joint>
</robot>

2
data/cube_small.urdf
View File

@@ -3,7 +3,7 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<rolling_friction value="0.3"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>

4
data/kiva_shelf/model.sdf
View File

@@ -2,10 +2,10 @@
<sdf version="1.4">
<model name="Amazon Pod">
<static>1</static>
<pose>0 2 1.21 0 0 0</pose>
<pose>0 2 0 0 0 0</pose>
<link name="pod_link">
<inertial>
<pose>0.0 0.0 1.2045 0 0 0</pose>
<pose>0.0 .0 1.2045 0 0 0</pose>
<mass>76.26</mass>
<inertia>
<ixx>47</ixx>

3
data/plane.urdf
View File

@@ -1,6 +1,9 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

29
data/sphere2_rolling_friction.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="base_link">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
</robot>

4
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -216,6 +216,10 @@ SET(BulletExampleBrowser_SRCS
../RoboticsLearning/b3RobotSimAPI.h
../RoboticsLearning/R2D2GraspExample.cpp
../RoboticsLearning/R2D2GraspExample.h
../RoboticsLearning/KukaGraspExample.cpp
../RoboticsLearning/KukaGraspExample.h
../RoboticsLearning/IKTrajectoryHelper.cpp
../RoboticsLearning/IKTrajectoryHelper.h
../RenderingExamples/CoordinateSystemDemo.cpp
../RenderingExamples/CoordinateSystemDemo.h
../RenderingExamples/RaytracerSetup.cpp

6
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -46,6 +46,7 @@
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
#include "../RoboticsLearning/R2D2GraspExample.h"
#include "../RoboticsLearning/KukaGraspExample.h"
#include "../RoboticsLearning/GripperGraspExample.h"
#include "../InverseKinematics/InverseKinematicsExample.h"
@@ -260,8 +261,11 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"Kuka IK","Control a Kuka IIWA robot to follow a target using IK. This IK is not setup properly yet.", KukaGraspExampleCreateFunc,0),
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc),
ExampleEntry(1,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION),
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),

2
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -232,7 +232,7 @@ void ImportSDFSetup::initPhysics()
MyMultiBodyCreator creation(m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);
mb = creation.getBulletMultiBody();

26
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -1,4 +1,3 @@
#include "URDFImporterInterface.h"
#include <stdio.h>
#include "LinearMath/btTransform.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -7,6 +6,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDF2Bullet.h"
#include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h"
#include <string>
@@ -143,7 +143,12 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
}
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false)
void ConvertURDF2BulletInternal(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix,
int flags = 0)
{
//b3Printf("start converting/extracting data from URDF interface\n");
@@ -200,7 +205,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
if (useSDF)
if (flags & CUF_USE_SDF)
{
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
}
@@ -231,7 +236,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
*/
if (mass)
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
if (!(flags & CUF_USE_URDF_INERTIA))
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF
@@ -436,18 +444,22 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF);
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
}
}
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false)
void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix, int flags)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF);
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
if (world1 && cache.m_bulletMultiBody)
{

15
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -14,14 +14,19 @@ class MultiBodyCreationInterface;
enum ConvertURDFFlags {
CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2
};
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
bool createMultiBody,
const char* pathPrefix,
bool useSDF = false);
int flags = 0);
#endif //_URDF2BULLET_H

21
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -646,6 +646,27 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
}
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
{
logger->reportError("Link/contact: rolling friction element must have value attribute");
return false;
}
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
}
}
}
}

10
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@@ -21,7 +21,7 @@
#define MAX_NUM_EFFECT 100
double T = 0;
VectorR3 target1[MAX_NUM_EFFECT];
VectorR3 targetaa[MAX_NUM_EFFECT];
@@ -85,7 +85,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian)
void UpdateTargets( double T2, Tree & treeY) {
double T = T2 / 5.;
target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
targetaa[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
}
@@ -103,7 +103,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
else {
jacob->SetJendActive();
}
jacob->ComputeJacobian(); // Set up Jacobian and deltaS vectors
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
@@ -129,7 +129,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
if ( SleepCounter==0 ) {
jacob->UpdateThetas(); // Apply the change in the theta values
jacob->UpdatedSClampValue();
jacob->UpdatedSClampValue(targetaa);
SleepCounter = SleepsPerStep;
}
else {
@@ -290,7 +290,7 @@ public:
getLocalTransform(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act);
b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z);
b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z);
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);

89
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -65,6 +65,7 @@ public:
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
if ((m_options & eGRIPPER_GRASP)!=0)
{
{
@@ -87,7 +88,7 @@ public:
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = false;
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
@@ -155,7 +156,63 @@ public:
//m_robotSim.setNumSimulationSubSteps(4);
}
if ((m_options & eTWO_POINT_GRASP)!=0)
{
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_gripper_left.urdf";
args.m_startPosition.setValue(0.068, 0.02, 0.11);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = -0.1;
controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(1,0,controlArgs);
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_gripper_right.urdf";
args.m_startPosition.setValue(-0.068, 0.02, 0.11);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = 0.1;
controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(2,0,controlArgs);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.setNumSimulationSubSteps(4);
}
}
virtual void exitPhysics()
@@ -164,22 +221,26 @@ public:
}
virtual void stepSimulation(float deltaTime)
{
if ((m_gripperIndex>=0))
if ((m_options & eGRIPPER_GRASP)!=0)
{
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
,-sGripperClosingTargetVelocity
};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
if ((m_gripperIndex>=0))
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
,-sGripperClosingTargetVelocity
};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()

5
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -16,6 +16,11 @@ subject to the following restrictions:
#ifndef GRIPPER_GRASP_EXAMPLE_H
#define GRIPPER_GRASP_EXAMPLE_H
enum GripperGraspExampleOptions
{
eGRIPPER_GRASP=1,
eTWO_POINT_GRASP=2,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

145
examples/RoboticsLearning/IKTrajectoryHelper.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "IKTrajectoryHelper.h"
#include "BussIK/Node.h"
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
}
};
IKTrajectoryHelper::IKTrajectoryHelper()
{
m_data = new IKTrajectoryHelperInternalData;
}
IKTrajectoryHelper::~IKTrajectoryHelper()
{
delete m_data;
}
void IKTrajectoryHelper::createKukaIIWA()
{
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_data->m_ikNodes.resize(8);//7DOF+additional endeffector
m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]);
m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]);
m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]);
m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]);
m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]);
m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]);
m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]);
m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]);
m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikTree.Init();
m_data->m_ikTree.Compute();
m_data->m_ikJacobian->Reset();
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod)
{
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive();
}
else {
m_data->m_ikJacobian->SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
}
m_data->m_ikJacobian->UpdateThetas();
// Apply the change in the theta values
m_data->m_ikJacobian->UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}

30
examples/RoboticsLearning/IKTrajectoryHelper.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef IK_TRAJECTORY_HELPER_H
#define IK_TRAJECTORY_HELPER_H
enum IK2_Method
{
IK2_JACOB_TRANS=0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD
};
class IKTrajectoryHelper
{
struct IKTrajectoryHelperInternalData* m_data;
public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool computeIK(const double endEffectorTargetPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod);
};
#endif //IK_TRAJECTORY_HELPER_H

243
examples/RoboticsLearning/KukaGraspExample.cpp Normal file
View File

@@ -0,0 +1,243 @@
#include "KukaGraspExample.h"
#include "IKTrajectoryHelper.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class KukaGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_kukaIndex;
IKTrajectoryHelper m_ikHelper;
int m_targetSphereInstance;
b3Vector3 m_targetPos;
double m_time;
int m_options;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
KukaGraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_kukaIndex(-1),
m_time(0)
{
m_targetPos.setValue(0.5,0,1);
m_app->setUpAxis(2);
}
virtual ~KukaGraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
}
m_app->m_renderer->writeTransforms();
m_ikHelper.createKukaIIWA();
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kuka_iiwa/model.urdf";
args.m_startPosition.setValue(0,0,0);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_kukaIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
}
*/
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
args.m_startOrientation = b3Quaternion(0,0,0,1);
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,0));
}
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_time+=deltaTime;
m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time));
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
//skip the base positions (7 values)
b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
for (int i=0;i<numJoints;i++)
{
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
double q_new[7];
int ikMethod=IK2_SDLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
q_new[3],q_new[4],q_new[5],q_new[6]);
//set the
for (int i=0;i<numJoints;i++)
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = q_new[i];
t.m_maxTorqueValue = 1000;
t.m_kp= 1;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance);
m_app->m_renderer->writeTransforms();
//draw the end-effector target sphere
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 3;
float pitch = -75;
float yaw = 30;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new KukaGraspExample(options.m_guiHelper, options.m_option);
}

27
examples/RoboticsLearning/KukaGraspExample.h Normal file
View File

@@ -0,0 +1,27 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef KUKA_GRASP_EXAMPLE_H
#define KUKA_GRASP_EXAMPLE_H
enum KukaGraspExampleOptions
{
eKUKA_GRASP_DLS_IK=1,
};
class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //KUKA_GRASP_EXAMPLE_H

37
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -94,11 +94,9 @@ public:
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_startPosition.setValue(0,0,.5);
args.m_startOrientation = b3Quaternion(0,B3_HALF_PI,0);
args.m_forceOverrideFixedBase = true;
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
//args.m_startOrientation = b3Quaternion()
args.m_startOrientation = b3Quaternion(0,0,0,1);
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
@@ -142,6 +140,37 @@ public:
}
}
if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "sphere2_rolling_friction.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args,results);
}
{
args.m_fileName = "sphere2.urdf";
args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_useMultiBody = true;
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
}
virtual void exitPhysics()

1
examples/RoboticsLearning/R2D2GraspExample.h
View File

@@ -20,6 +20,7 @@ enum RobotLearningExampleOptions
{
eROBOTIC_LEARN_GRASP=1,
eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
eROBOTIC_LEARN_ROLLING_FRICTION=4,
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);

54
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -615,6 +615,60 @@ void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int c
}
}
bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state)
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
if (statusHandle)
{
double rootInertialFrame[7];
const double* rootLocalInertialFrame;
const double* actualStateQ;
const double* actualStateQdot;
const double* jointReactionForces;
int stat = b3GetStatusActualState(statusHandle,
&state.m_bodyUniqueId,
&state.m_numDegreeOfFreedomQ,
&state.m_numDegreeOfFreedomU,
&rootLocalInertialFrame,
&actualStateQ,
&actualStateQdot,
&jointReactionForces);
if (stat)
{
state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
{
state.m_actualStateQ[i] = actualStateQ[i];
}
for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
{
state.m_actualStateQdot[i] = actualStateQdot[i];
}
int numJoints = getNumJoints(bodyUniqueId);
state.m_jointReactionForces.resize(6*numJoints);
for (int i=0;i<numJoints*6;i++)
{
state.m_jointReactionForces[i] = jointReactionForces[i];
}
return true;
}
//rootInertialFrame,
// &state.m_actualStateQ[0],
// &state.m_actualStateQdot[0],
// &state.m_jointReactionForces[0]);
}
return false;
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;

15
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -6,6 +6,7 @@
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
@@ -17,6 +18,17 @@ enum b3RigidSimFileType
B3_AUTO_DETECT_FILE//todo based on extension
};
struct b3JointStates
{
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
b3Transform m_rootLocalInertialFrame;
b3AlignedObjectArray<double> m_actualStateQ;
b3AlignedObjectArray<double> m_actualStateQdot;
b3AlignedObjectArray<double> m_jointReactionForces;
};
struct b3RobotSimLoadFileArgs
{
std::string m_fileName;
@@ -32,6 +44,7 @@ struct b3RobotSimLoadFileArgs
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_fileType(B3_URDF_FILE)
{
}
@@ -90,6 +103,8 @@ public:
void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointStates(int bodyUniqueId, b3JointStates& state);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -40,6 +40,8 @@ public:
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
};
#endif // BT_PHYSICS_CLIENT_API_H

45
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1103,6 +1103,51 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION;
command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
}
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedContactPointInformation(contactPointData);
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -80,6 +80,11 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command,
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);

23
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -274,7 +274,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5);
b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
@@ -415,6 +415,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
if (m_selectedBody >= 0)
@@ -442,6 +443,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
b3SetContactFilterBodyA(commandHandle,0);
b3SetContactFilterBodyB(commandHandle,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -530,6 +539,7 @@ void PhysicsClientExample::createButtons()
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
if (m_bodyUniqueIds.size())
{
@@ -946,6 +956,17 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
}
if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED)
{
b3Warning("Cannot get contact information");
}
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
b3ContactInformation contactPointData;
b3GetContactPointInformation(m_physicsClientHandle, &contactPointData);
b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints);
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))

50
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -38,6 +38,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -58,6 +60,7 @@ struct PhysicsClientSharedMemoryInternalData {
m_counter(0),
m_cachedCameraPixelsWidth(0),
m_cachedCameraPixelsHeight(0),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -558,6 +561,32 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Inverse Dynamics computations failed");
break;
}
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0;i<numContactsCopied;i++)
{
m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
}
break;
}
case CMD_CONTACT_POINT_INFORMATION_FAILED:
{
b3Warning("Contact Point Information Request failed");
break;
}
default: {
b3Error("Unknown server status\n");
btAssert(0);
@@ -620,6 +649,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -716,6 +759,13 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
}
void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -48,6 +48,8 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};
#endif // BT_PHYSICS_CLIENT_API_H

59
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -39,6 +39,8 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMask;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
PhysicsServerCommandProcessor* m_commandProcessor;
@@ -193,6 +195,51 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i=0;i<numContactsCopied;i++)
{
m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
}
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
}
}
} while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
return m_data->m_hasStatus;
}
bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
@@ -326,6 +373,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
{
return processCamera(command);
}
if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION)
{
return processContactPointData(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
@@ -487,3 +538,11 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
}
}
void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
}

4
examples/SharedMemory/PhysicsDirect.h
View File

@@ -21,6 +21,8 @@ protected:
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
@@ -64,6 +66,8 @@ public:
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();

7
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -105,10 +105,12 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const
{
return m_data->m_physicsClient->getDebugLinesFrom();
}
const float* PhysicsLoopBack::getDebugLinesTo() const
{
return m_data->m_physicsClient->getDebugLinesTo();
}
const float* PhysicsLoopBack::getDebugLinesColor() const
{
return m_data->m_physicsClient->getDebugLinesColor();
@@ -118,3 +120,8 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
return m_data->m_physicsClient->getCachedCameraImage(cameraData);
}
void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
}

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -53,6 +53,8 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};

188
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -403,6 +403,8 @@ struct PhysicsServerCommandProcessorInternalData
btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -507,7 +509,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
}
m_data->m_guiHelper = guiHelper;
}
@@ -518,7 +520,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
}
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -759,6 +761,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
btScalar mass = 0;
@@ -776,15 +780,22 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true);
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);
mb = creation.getBulletMultiBody();
rb = creation.getRigidBody();
if (rb)
rb->setUserIndex2(bodyUniqueId);
if (mb)
mb->setUserIndex2(bodyUniqueId);
if (mb)
{
bodyHandle->m_multiBody = mb;
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
@@ -856,6 +867,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{
btScalar mass = 0;
@@ -887,14 +900,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
if (useMultiBody)
{
if (mb)
{
mb->setUserIndex2(bodyUniqueId);
bodyHandle->m_multiBody = mb;
createJointMotors(mb);
@@ -958,6 +971,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (rb)
{
bodyHandle->m_rigidBody = rb;
rb->setUserIndex2(bodyUniqueId);
return true;
}
}
@@ -1024,7 +1038,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
bool hasStatus = false;
{
@@ -1154,7 +1168,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
int numPixelsCopied = 0;
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
{
@@ -1481,10 +1495,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
}
}
motor->setVelocityTarget(desiredVelocity,kd);
btScalar kp = 0.f;
motor->setPositionTarget(0,kp);
hasDesiredVelocity = true;
@@ -1565,7 +1579,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity,kd);
motor->setPositionTarget(desiredPosition,kp);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
@@ -1805,7 +1819,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
if (m_data->m_verboseOutput)
{
b3Printf("Step simulation request");
@@ -1897,7 +1911,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
mb->setWorldToBaseRot(invOrn.inverse());
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
@@ -1936,10 +1950,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
@@ -2079,6 +2093,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
rb->setUserIndex2(bodyUniqueId);
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
hasStatus = true;
@@ -2121,6 +2136,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
//make a snapshot of the contact manifolds into individual contact points
if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
{
int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
m_data->m_cachedContactPoints.resize(0);
m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
for (int i=0;i<numContactManifolds;i++)
{
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
int linkIndexA = -1;
int linkIndexB = -1;
int objectIndexB = -1;
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
if (bodyB)
{
objectIndexB = bodyB->getUserIndex2();
}
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (mblB && mblB->m_multiBody)
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
int objectIndexA = -1;
const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
if (bodyA)
{
objectIndexA = bodyA->getUserIndex2();
}
const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2();
}
btAssert(bodyA || mblA);
//apply the filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
continue;
}
//apply the second object filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
(clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
continue;
}
for (int p=0;p<manifold->getNumContacts();p++)
{
b3ContactPointData pt;
pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
const btManifoldPoint& srcPt = manifold->getContactPoint(p);
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j=0;j<3;j++)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_data->m_cachedContactPoints.push_back (pt);
}
}
}
int numContactPoints = m_data->m_cachedContactPoints.size();
//b3ContactPoint
//struct b3ContactPointDynamics
int totalBytesPerContact = sizeof(b3ContactPointData);
int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1;
b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
int numContactPointBatch = btMin(numContactPoints,contactPointStorage);
int endContactPointIndex = startContactPointIndex+numContactPointBatch;
for (int i=startContactPointIndex;i<endContactPointIndex ;i++)
{
const b3ContactPointData& srcPt = m_data->m_cachedContactPoints[i];
b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
destPt = srcPt;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
}
serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
hasStatus = true;
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
@@ -2128,7 +2261,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
@@ -2140,12 +2273,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
else
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
@@ -2163,8 +2296,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
@@ -2179,7 +2315,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
}
}
else
{
@@ -2203,7 +2339,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
@@ -2213,7 +2349,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_externalForceArguments.m_positions[i*3+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
@@ -2236,7 +2372,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
@@ -2252,7 +2388,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
@@ -2514,7 +2650,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
}
}

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -144,6 +144,12 @@ enum EnumRequestPixelDataUpdateFlags
};
struct RequestContactDataArgs
{
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
};
struct SendDebugLinesArgs
@@ -406,6 +412,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
};
};
@@ -414,6 +421,13 @@ struct RigidBodyCreateArgs
int m_bodyUniqueId;
};
struct SendContactDataArgs
{
int m_startingContactPointIndex;
int m_numContactPointsCopied;
int m_numRemainingContactPoints;
};
struct SharedMemoryStatus
{
int m_type;
@@ -430,6 +444,7 @@ struct SharedMemoryStatus
struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
};
};

40
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -28,8 +28,12 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_CALCULATE_INVERSE_KINEMATICS,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
CMD_CREATE_JOINT
};
enum EnumSharedMemoryServerStatus
@@ -63,6 +67,8 @@ enum EnumSharedMemoryServerStatus
CMD_INVALID_STATUS,
CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_MAX_SERVER_COMMANDS
};
@@ -130,6 +136,38 @@ struct b3CameraImageData
const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
};
struct b3ContactPointData
{
//todo: expose some contact flags, such as telling which fields below are valid
int m_contactFlags;
int m_bodyUniqueIdA;
int m_bodyUniqueIdB;
int m_linkIndexA;
int m_linkIndexB;
double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
double m_contactDistance;//negative number is penetration, positive is distance.
double m_normalForce;
//todo: expose the friction forces as well
// double m_linearFrictionDirection0[3];
// double m_linearFrictionForce0;
// double m_linearFrictionDirection1[3];
// double m_linearFrictionForce1;
// double m_angularFrictionDirection[3];
// double m_angularFrictionForce;
};
struct b3ContactInformation
{
int m_numContactPoints;
struct b3ContactPointData* m_contactPointData;
};
///b3LinkState provides extra information such as the Cartesian world coordinates
///center of mass (COM) of the link, relative to the world reference frame.
///Orientation is a quaternion x,y,z,w

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

@@ -36,7 +36,7 @@ using namespace std;
void Arrow(const VectorR3& tail, const VectorR3& head);
//extern RestPositionOn;
extern VectorR3 target1[];
//extern VectorR3 target1[];
// Optimal damping values have to be determined in an ad hoc manner (Yuck!)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
@@ -96,7 +96,7 @@ void Jacobian::Reset()
// Compute the deltaS vector, dS, (the error in end effector positions
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian()
void Jacobian::ComputeJacobian(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@@ -104,7 +104,7 @@ void Jacobian::ComputeJacobian()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
temp = targetPos;
@@ -418,7 +418,7 @@ void Jacobian::CalcdTClampedFromdS()
}
}
double Jacobian::UpdateErrorArray()
double Jacobian::UpdateErrorArray(VectorR3* targets)
{
double totalError = 0.0;
@@ -428,7 +428,7 @@ double Jacobian::UpdateErrorArray()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
temp = targetPos;
temp -= n->GetS();
double err = temp.Norm();
@@ -440,7 +440,7 @@ double Jacobian::UpdateErrorArray()
return totalError;
}
void Jacobian::UpdatedSClampValue()
void Jacobian::UpdatedSClampValue(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@@ -448,7 +448,7 @@ void Jacobian::UpdatedSClampValue()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
// While we are at it, also update the clamping values in dSclamp;

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

@@ -55,7 +55,7 @@ class Jacobian {
public:
Jacobian(Tree*);
void ComputeJacobian();
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
@@ -69,9 +69,9 @@ public:
void CalcDeltaThetasSDLS();
void UpdateThetas();
double UpdateErrorArray(); // Returns sum of errors
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue();
void UpdatedSClampValue(VectorR3* targets);
void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }

6
examples/TinyRenderer/tgaimage.h
View File

@@ -30,20 +30,20 @@ struct TGAColor {
bgra[i] = 0;
}
TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) {
TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) {
bgra[0] = B;
bgra[1] = G;
bgra[2] = R;
bgra[3] = A;
}
TGAColor(unsigned char v) : bgra(), bytespp(1) {
TGAColor(unsigned char v) : bytespp(1) {
for (int i=0; i<4; i++) bgra[i] = 0;
bgra[0] = v;
}
TGAColor(const unsigned char *p, unsigned char bpp) : bgra(), bytespp(bpp) {
TGAColor(const unsigned char *p, unsigned char bpp) : bytespp(bpp) {
for (int i=0; i<(int)bpp; i++) {
bgra[i] = p[i];
}

122
examples/pybullet/pybullet.c
View File

@@ -617,14 +617,16 @@ static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return 0;
}
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
b3GetStatusActualState(status_handle, 0/* body_unique_id */,
0/* num_degree_of_freedom_q */,
0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,
@@ -1111,6 +1113,116 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
return 0;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args)
{
int size= PySequence_Size(args);
int objectUniqueIdA = -1;
int objectUniqueIdB = -1;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* pyResultList=0;
if (size==1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
if (size==2)
{
if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle,objectUniqueIdA);
b3SetContactFilterBodyB(commandHandle,objectUniqueIdB);
b3SubmitClientCommand(sm, commandHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
14 double m_contactDistance;//negative number is penetration, positive is distance.
15 double m_normalForce;
*/
b3GetContactPointInformation(sm, &contactPointData);
pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
for (int i=0;i<contactPointData.m_numContactPoints;i++)
{
PyObject* contactObList = PyTuple_New(16);//see above 16 fields
PyObject* item;
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
PyTuple_SetItem(contactObList,0,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
PyTuple_SetItem(contactObList,1,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
PyTuple_SetItem(contactObList,2,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
PyTuple_SetItem(contactObList,3,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList,4,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList,5,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList,6,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList,7,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList,8,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList,9,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList,10,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList,11,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList,12,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList,13,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList,14,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList,15,item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
return pyResultList;
}
Py_INCREF(Py_None);
return Py_None;
}
// Render an image from the current timestep of the simulation
//
// Examples:
@@ -1792,6 +1904,10 @@ static PyMethodDef SpamMethods[] = {
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
"Return the contact point information for all or some of pairwise object-object collisions. Optional arguments one or two object unique ids, that need to be involved in the contact."},
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},

1
src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
View File

@@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject()
m_rollingFriction(0.0f),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),

17
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -93,8 +93,10 @@ protected:
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
void* m_userObjectPointer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
///time of impact calculation
@@ -476,6 +478,12 @@ public:
{
return m_userIndex;
}
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
@@ -487,6 +495,11 @@ public:
{
m_userIndex = index;
}
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
int getUpdateRevisionInternal() const
{

4
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -1053,8 +1053,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
//disabled: only a single rollingFriction per manifold
// rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();

4
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links,
m_awake(true),
m_canSleep(canSleep),
m_sleepTimer(0),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),

36
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -577,6 +577,38 @@ void addJointTorque(int i, btScalar Q);
m_baseName = name;
}
///users can point to their objects, userPointer is not used by Bullet
void* getUserPointer() const
{
return m_userObjectPointer;
}
int getUserIndex() const
{
return m_userIndex;
}
int getUserIndex2() const
{
return m_userIndex2;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
m_userObjectPointer = userPointer;
}
///users can point to their objects, userPointer is not used by Bullet
void setUserIndex(int index)
{
m_userIndex = index;
}
void setUserIndex2(int index)
{
m_userIndex2 = index;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -653,6 +685,10 @@ private:
bool m_canSleep;
btScalar m_sleepTimer;
void* m_userObjectPointer;
int m_userIndex2;
int m_userIndex;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;

336
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -534,8 +534,285 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& constraintNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
btVector3 rel_pos1;
btVector3 rel_pos2;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = infoGlobal.m_sor;
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
cfm *= invTimeStep;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = pos1 - multiBodyA->getBasePos();
} else
{
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
solverConstraint.m_jacAindex = m_data.m_jacobians.size();
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
} else
{
btVector3 torqueAxis0 = constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0,0,0);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = pos2 - multiBodyB->getBasePos();
} else
{
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
}
solverConstraint.m_jacBindex = m_data.m_jacobians.size();
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = constraintNormal;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
} else
{
btVector3 torqueAxis1 = constraintNormal;
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
} else
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + constraintNormal.dot(vec);
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
} else
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + constraintNormal.dot(vec);
}
}
btScalar d = denom0+denom1+cfm;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = cp.m_combinedRollingFriction;
if(!isFriction)
{
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
}
}
}
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
if (penetration>0)
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
}
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -572,6 +849,41 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
return solverConstraint;
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyRollingFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_multiBodyA = mbA;
if (mbA)
solverConstraint.m_linkA = fcA->m_link;
solverConstraint.m_multiBodyB = mbB;
if (mbB)
solverConstraint.m_linkB = fcB->m_link;
solverConstraint.m_originalContactPoint = &cp;
setupMultiBodyRollingFrictionConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
return solverConstraint;
}
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
@@ -596,8 +908,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
// return;
//only a single rollingFriction per manifold
int rollingFriction=1;
for (int j=0;j<manifold->getNumContacts();j++)
{
@@ -650,8 +963,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
//disabled: only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
@@ -679,9 +992,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#endif //ROLLING_FRICTION
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///By default, each contact has only a single friction direction that is recomputed automatically every frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///If the relative velocity is zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
@@ -722,6 +1035,15 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if (rollingFriction > 0)
{
addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
rollingFriction--;
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{

10
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -47,8 +47,10 @@ protected:
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btMultiBodySolverConstraint& addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
@@ -60,6 +62,12 @@ protected:
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);