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

Merge pull request #2580 from erwincoumans/master

Browse Source 
add --mp4fps=30 command line parameter for ExampleBrowser, various other fixes
This commit is contained in:
erwincoumans
2020-01-11 15:14:11 -08:00
committed by GitHub
parent 61fa943994 8b578093a3
commit a9615258d3
15 changed files with 216 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -44,6 +44,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
MatrixRmn AugMat;
bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, 1);
@@ -142,12 +143,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
@@ -193,6 +194,7 @@ bool IKTrajectoryHelper::computeIK2(
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
{
MatrixRmn AugMat;
bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
@@ -250,12 +252,12 @@ bool IKTrajectoryHelper::computeIK2(
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();

16
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -789,22 +789,6 @@ struct CalculateInverseKinematicsResultArgs
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
};
enum EnumUserConstraintFlags
{
USER_CONSTRAINT_ADD_CONSTRAINT = 1,
USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
USER_CONSTRAINT_REQUEST_INFO = 64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
USER_CONSTRAINT_CHANGE_ERP = 1024,
USER_CONSTRAINT_REQUEST_STATE = 2048,
USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
};
enum EnumBodyChangeFlags
{

17
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -309,6 +309,23 @@ struct b3UserDataValue
const char* m_data1;
};
enum EnumUserConstraintFlags
{
USER_CONSTRAINT_ADD_CONSTRAINT = 1,
USER_CONSTRAINT_REMOVE_CONSTRAINT = 2,
USER_CONSTRAINT_CHANGE_CONSTRAINT = 4,
USER_CONSTRAINT_CHANGE_PIVOT_IN_B = 8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B = 16,
USER_CONSTRAINT_CHANGE_MAX_FORCE = 32,
USER_CONSTRAINT_REQUEST_INFO = 64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO = 128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK = 256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
USER_CONSTRAINT_CHANGE_ERP = 1024,
USER_CONSTRAINT_REQUEST_STATE = 2048,
USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
};
struct b3UserConstraint
{
int m_parentBodyIndex;

41
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -620,7 +620,7 @@ int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, in
return -1;
}
int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo)
int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3RobotUserConstraint* jointInfo)
{
if (!isConnected())
{
@@ -629,16 +629,35 @@ int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3Joi
}
b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
if (jointInfo->m_flags & eJointChangeMaxForce)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_maxAppliedForce);
}
if (jointInfo->m_flags & eJointChangeChildFramePosition)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
b3InitChangeUserConstraintSetGearRatio(commandHandle, jointInfo->m_gearRatio);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
b3InitChangeUserConstraintSetERP(commandHandle, jointInfo->m_erp);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
b3InitChangeUserConstraintSetGearAuxLink(commandHandle, jointInfo->m_gearAuxLink);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, jointInfo->m_relativePositionTarget);
}
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
}
if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
}
@@ -1131,7 +1150,7 @@ void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double camer
}
}
void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName)
{
if (!isConnected())
{
@@ -1140,10 +1159,16 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
}
b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
if (durationInMicroSeconds >= 0)
if (profileName.length())
{
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
b3SetProfileTimingType(commandHandle, 0);
}
else
{
b3SetProfileTimingType(commandHandle, 1);
}
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

98
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -526,6 +526,100 @@ struct b3RobotSimulatorCreateMultiBodyArgs
}
};
struct b3RobotUserConstraint : public b3UserConstraint
{
int m_userUpdateFlags;//see EnumUserConstraintFlags
void setErp(double erp)
{
m_erp = erp;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_ERP;
}
void setMaxAppliedForce(double maxForce)
{
m_maxAppliedForce = maxForce;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE;
}
void setGearRatio(double gearRatio)
{
m_gearRatio = gearRatio;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO;
}
void setGearAuxLink(int link)
{
m_gearAuxLink = link;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
}
void setRelativePositionTarget(double target)
{
m_relativePositionTarget = target;
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET;
}
void setChildPivot(double pivot[3])
{
m_childFrame[0] = pivot[0];
m_childFrame[1] = pivot[1];
m_childFrame[2] = pivot[2];
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
}
void setChildFrameOrientation(double orn[4])
{
m_childFrame[3] = orn[0];
m_childFrame[4] = orn[1];
m_childFrame[5] = orn[2];
m_childFrame[6] = orn[3];
m_userUpdateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
}
b3RobotUserConstraint()
:m_userUpdateFlags(0)
{
m_parentBodyIndex = -1;
m_parentJointIndex = -1;
m_childBodyIndex = -1;
m_childJointIndex = -1;
//position
m_parentFrame[0] = 0;
m_parentFrame[1] = 0;
m_parentFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_parentFrame[3] = 0;
m_parentFrame[4] = 0;
m_parentFrame[5] = 0;
m_parentFrame[6] = 1;
//position
m_childFrame[0] = 0;
m_childFrame[1] = 0;
m_childFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_childFrame[3] = 0;
m_childFrame[4] = 0;
m_childFrame[5] = 0;
m_childFrame[6] = 1;
m_jointAxis[0] = 0;
m_jointAxis[1] = 0;
m_jointAxis[2] = 1;
m_jointType = eFixedType;
m_maxAppliedForce = 500;
m_userConstraintUniqueId = -1;
m_gearRatio = -1;
m_gearAuxLink = -1;
m_relativePositionTarget = 0;
m_erp = 0;
}
};
struct b3RobotJointInfo : public b3JointInfo
{
b3RobotJointInfo()
@@ -624,7 +718,7 @@ public:
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3JointInfo *jointInfo);
int changeConstraint(int constraintId, b3RobotUserConstraint*jointInfo);
void removeConstraint(int constraintId);
@@ -670,7 +764,7 @@ public:
void getVREvents(b3VREventsData *vrEventsData, int deviceTypeFilter);
void getKeyboardEvents(b3KeyboardEventsData *keyboardEventsData);
void submitProfileTiming(const std::string &profileName, int durationInMicroSeconds = 1);
void submitProfileTiming(const std::string &profileName);
// JFC: added these 24 methods