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

add experimental pr2-gripper support in VR physics server

Browse Source 
add setErp to btMultiBodyJointMotor
This commit is contained in:
erwin coumans
2016-09-09 14:30:37 -07:00
parent f72982306e
commit 3c706306cd
7 changed files with 234 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
build_visual_studio.bat
View File

@@ -2,9 +2,9 @@
rem premake4 --with-pe vs2010
rem premake4 --bullet2demos vs2010
cd build3
premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_pybullet --python_include_dir="c:/python-3.5.2/include" --python_lib_dir="c:/python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010
premake4 --enable_pybullet --python_include_dir="c:/python34/include" --python_lib_dir="c:/python34/libs" --enable_openvr --targetdir="../bin" vs2010
rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010
rem premake4 --targetdir="../server2bin" vs2010
rem cd vs2010

126
data/pr2_gripper.urdf Normal file
View File

@@ -0,0 +1,126 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
<material name="Gray">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
</robot>

84
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -382,6 +382,7 @@ struct PhysicsServerCommandProcessorInternalData
bool m_hasGround;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -432,6 +433,7 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(true),
m_commandLogger(0),
m_logPlayback(0),
@@ -2697,6 +2699,9 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1);
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
@@ -2717,37 +2722,70 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
if (0)//m_data->m_gripperRigidbodyFixed==0)
{
int bodyId = 0;
loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
if (m_data->m_gripperRigidbodyFixed == 0)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0,0,0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0,0,0,1));
btVector3 pivotInChild(0,0,0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
int bodyId = 0;
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild);
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0, 0, 0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
}
if (m_data->m_gripperRigidbodyFixed)
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
for (int i = 0; i < 2; i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
{
motor->setErp(0.005);
if (gVRGripperClosed)
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
}
int maxSteps = 3;

41
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -18,6 +18,8 @@
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
extern bool gVRGripperClosed;
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
@@ -1116,20 +1118,29 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
gDebugRenderToggle = !gDebugRenderToggle;
}
if (button==1)
{
m_args[0].m_isVrControllerTeleporting[controllerId] = true;
}
if ((button==33))
if (controllerId == 3 && (button == 33))
{
m_args[0].m_isVrControllerPicking[controllerId] = (state!=0);
m_args[0].m_isVrControllerReleasing[controllerId] = (state==0);
gVRGripperClosed =state;
}
if ((button==33)||(button==1))
else
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
if ((button == 33))
{
m_args[0].m_isVrControllerPicking[controllerId] = (state != 0);
m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
}
if ((button == 33) || (button == 1))
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
}
@@ -1144,13 +1155,17 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
return;
}
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]);
gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0],orn[1],orn[2],orn[3]);
gVRGripperOrn = orgOrn*btQuaternion(btVector3(0,0,1),SIMD_HALF_PI)*btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
if (controllerId == 3)
{
gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
}
else
{
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

6
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -700,12 +700,16 @@ bool CMainApplication::HandleInput()
if (button==2)
{
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
//add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
}
else
{

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

@@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(1.),
m_kp(0)
m_kp(0),
m_erp(1)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -123,7 +124,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;

9
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -29,6 +29,7 @@ protected:
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
public:
@@ -57,6 +58,14 @@ public:
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{