From 5cd46479431120271c9f1a9a2fd75ede08dfa2f4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 9 Jan 2020 10:40:14 -0800 Subject: [PATCH 1/6] fix submitProfileTiming API in b3RobotSimulatorClientAPI_NoDirect --- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 12 +++++++++--- .../b3RobotSimulatorClientAPI_NoDirect.h | 2 +- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 0a2741d7b..9f04cf6ce 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -1131,7 +1131,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 +1140,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); } diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index dc039974e..c23a7af0d 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -670,7 +670,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 From 8ebdf7862c1a36d178eb7e8c0590016505b52ba7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 9 Jan 2020 17:56:21 -0800 Subject: [PATCH 2/6] fix b3RobotSimulatorClientAPI_NoDirect::changeConstraint API --- examples/SharedMemory/SharedMemoryCommands.h | 16 ---- examples/SharedMemory/SharedMemoryPublic.h | 17 ++++ .../b3RobotSimulatorClientAPI_NoDirect.cpp | 29 +++++- .../b3RobotSimulatorClientAPI_NoDirect.h | 96 ++++++++++++++++++- 4 files changed, 136 insertions(+), 22 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 958cf38a4..223a0a5d5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 01727fe5c..63a7cfa55 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 9f04cf6ce..4e81e599a 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -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]); } diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index c23a7af0d..78f729735 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -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); From 83bdef8254cd43e57f29dbc68cfae183e543f368 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 12:19:42 -0800 Subject: [PATCH 3/6] add --mp4fps=30 command line parameter for ExampleBrowser (and using pybullet.connect(p.GUI, options="--mp4fps=30 --mp4=\"testvideo.mp4\"") --- .../CommonGraphicsAppInterface.h | 1 + .../ExampleBrowser/OpenGLExampleBrowser.cpp | 7 +++++++ examples/OpenGLWindow/SimpleOpenGL3App.cpp | 18 +++++++++++++----- examples/OpenGLWindow/SimpleOpenGL3App.h | 1 + 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index acfaf478c..a3d9d3eac 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -93,6 +93,7 @@ struct CommonGraphicsApp if (blue) *blue = m_backgroundColorRGB[2]; } + virtual void setMp4Fps(int fps) {} virtual void setBackgroundColor(float red, float green, float blue) { m_backgroundColorRGB[0] = red; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index fcfb1c05a..a76cdfe5b 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -921,6 +921,13 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) m_internalData->m_app = s_app; char* gVideoFileName = 0; args.GetCmdLineArgument("mp4", gVideoFileName); + int gVideoFps = 0; + args.GetCmdLineArgument("mp4fps", gVideoFps); + if (gVideoFps) + { + simpleApp->setMp4Fps(gVideoFps); + } + #ifndef NO_OPENGL3 if (gVideoFileName) simpleApp->dumpFramesToVideo(gVideoFileName); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 7293cc5b6..e3faf48d9 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -66,6 +66,8 @@ struct SimpleInternalData int m_upAxis; //y=1 or z=2 is supported int m_customViewPortWidth; int m_customViewPortHeight; + int m_mp4Fps; + SimpleInternalData() : m_fontTextureId(0), m_largeFontTextureId(0), @@ -82,7 +84,8 @@ struct SimpleInternalData m_userPointer(0), m_upAxis(1), m_customViewPortWidth(-1), - m_customViewPortHeight(-1) + m_customViewPortHeight(-1), + m_mp4Fps(60) { } }; @@ -1089,6 +1092,11 @@ void SimpleOpenGL3App::swapBuffer() m_window->startRendering(); } +void SimpleOpenGL3App::setMp4Fps(int fps) +{ + m_data->m_mp4Fps = fps; +} + // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) { @@ -1100,12 +1108,12 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) #ifdef _WIN32 sprintf(cmd, - "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " + "ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - " "-threads 0 -y -b:v 50000k -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", - width, height, mp4FileName); + m_data->m_mp4Fps, width, height, mp4FileName); - //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); + //sprintf(cmd, "ffmpeg -r %d -f rawvideo -pix_fmt rgba -s %dx%d -i - " + // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", m_data->m_mp4Fps, width, height, mp4FileName); #else sprintf(cmd, diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.h b/examples/OpenGLWindow/SimpleOpenGL3App.h index cc9573c29..c90f47387 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.h +++ b/examples/OpenGLWindow/SimpleOpenGL3App.h @@ -14,6 +14,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp class GLPrimitiveRenderer* m_primRenderer; class GLInstancingRenderer* m_instancingRenderer; virtual void setBackgroundColor(float red, float green, float blue); + virtual void setMp4Fps(int fps); SimpleOpenGL3App(const char* title, int width, int height, bool allowRetina = true, int windowType = 0, int renderDevice = -1, int maxNumObjectCapacity = 128 * 1024, int maxShapeCapacityInBytes = 128 * 1024 * 1024); From 2336dfcb9ec74909e37ddf769a2bdbf7160b0f27 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 12:43:27 -0800 Subject: [PATCH 4/6] Remove a temporary global static work matrix in the BussIK, since it conflicts with multithreaded applications. Instead, let the user pass it in explicitly. --- .../InverseKinematicsExample.cpp | 4 ++-- examples/SharedMemory/IKTrajectoryHelper.cpp | 10 ++++++---- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 16 ++++++++-------- examples/ThirdPartyLibs/BussIK/Jacobian.h | 8 ++++---- examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 7 ++++--- examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 9 +-------- 6 files changed, 25 insertions(+), 29 deletions(-) diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 9fea9ce35..a5333555d 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -100,7 +100,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod) jacob->SetJendActive(); } jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors - + MatrixRmn AugMat; // Calculate the change in theta values switch (ikMethod) { @@ -108,7 +108,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod) jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method break; case IK_DLS: - jacob->CalcDeltaThetasDLS(); // Damped least squares method + jacob->CalcDeltaThetasDLS(AugMat); // Damped least squares method break; case IK_DLS_SVD: jacob->CalcDeltaThetasDLSwithSVD(); diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 472a24beb..fb002cc3a 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -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(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index f91a5e397..4a728a072 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot() m_tree->Compute(); } -void Jacobian::CalcDeltaThetas() +void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat) { switch (CurrentUpdateMode) { @@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas() CalcDeltaThetasPseudoinverse(); break; case JACOB_DLS: - CalcDeltaThetasDLS(); + CalcDeltaThetasDLS(AugMat); break; case JACOB_SDLS: CalcDeltaThetasSDLS(); @@ -327,7 +327,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse() } } -void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) +void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) // J.MultiplyTranspose( dTextra, dTheta ); // Use these two lines for the traditional DLS method - U.Solve(dS, &dT1); + U.Solve(dS, &dT1, AugMat); J.MultiplyTranspose(dT1, dTheta); // Compute JInv in damped least square form @@ -379,7 +379,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) } } -void Jacobian::CalcDeltaThetasDLS() +void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS() // J.MultiplyTranspose( dTextra, dTheta ); // Use these two lines for the traditional DLS method - U.Solve(dS, &dT1); + U.Solve(dS, &dT1, AugMat); J.MultiplyTranspose(dT1, dTheta); // Scale back to not exceed maximum angle changes @@ -404,7 +404,7 @@ void Jacobian::CalcDeltaThetasDLS() } } -void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec) +void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec) dT1.SetLength(J.GetNumColumns()); J.MultiplyTranspose(dS, dT1); - U.Solve(dT1, &dTheta); + U.Solve(dT1, &dTheta, AugMat); // Scale back to not exceed maximum angle changes double maxChange = dTheta.MaxAbs(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index d7dc81bcd..915e56aa3 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -66,15 +66,15 @@ public: void SetJendTrans(MatrixRmn& J); void SetDeltaS(VectorRn& S); - void CalcDeltaThetas(); // Use this only if the Current Mode has been set. + void CalcDeltaThetas(MatrixRmn& AugMat); // Use this only if the Current Mode has been set. void ZeroDeltaThetas(); void CalcDeltaThetasTranspose(); void CalcDeltaThetasPseudoinverse(); - void CalcDeltaThetasDLS(); - void CalcDeltaThetasDLS2(const VectorRn& dVec); + void CalcDeltaThetasDLS(MatrixRmn& AugMat); + void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV); + void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat); void UpdateThetas(); void UpdateThetaDot(); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 4bf1e8771..c1c20cc83 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -30,7 +30,6 @@ subject to the following restrictions: #include "MatrixRmn.h" -MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix // Fill the diagonal entries with the value d. The rest of the matrix is unchanged. void MatrixRmn::SetDiagonalEntries(double d) @@ -354,12 +353,14 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B, // Solves the equation (*this)*xVec = b; // Uses row operations. Assumes *this is square and invertible. // No error checking for divide by zero or instability (except with asserts) -void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec) const +void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const { + B3_PROFILE("MatrixRmn::Solve"); assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength()); // Copy this matrix and b into an Augmented Matrix - MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1); + + AugMat.SetSize(NumRows, NumCols + 1); AugMat.LoadAsSubmatrix(*this); AugMat.SetColumn(NumRows, b); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 86e42678b..343e79b90 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -117,7 +117,7 @@ public: MatrixRmn& AddToDiagonal(const VectorRn& dVec); // Solving systems of linear equations - void Solve(const VectorRn& b, VectorRn* x) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible. + void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible. // Row Echelon Form and Reduced Row Echelon Form routines // Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables. @@ -150,13 +150,6 @@ private: double* x; // Array of vector entries - stored in column order long AllocSize; // Allocated size of the x array - static MatrixRmn WorkMatrix; // Temporary work matrix - static MatrixRmn& GetWorkMatrix() { return WorkMatrix; } - static MatrixRmn& GetWorkMatrix(long numRows, long numCols) - { - WorkMatrix.SetSize(numRows, numCols); - return WorkMatrix; - } // Internal helper routines for SVD calculations static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag); From 8f8bbbee3bbd281a190f8a11443d91f57f135f44 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 13:06:45 -0800 Subject: [PATCH 5/6] example how to create a video using PyBullet using GUI mode with ffmpeg, synchronizing video with stepSimulation at specific frame rate (240 Hz for example) --- examples/pybullet/examples/video_sync_mp4.py | 24 ++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 examples/pybullet/examples/video_sync_mp4.py diff --git a/examples/pybullet/examples/video_sync_mp4.py b/examples/pybullet/examples/video_sync_mp4.py new file mode 100644 index 000000000..ba8956bec --- /dev/null +++ b/examples/pybullet/examples/video_sync_mp4.py @@ -0,0 +1,24 @@ +import pybullet as p +import time + +#by default, PyBullet runs at 240Hz +p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240") +p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) +p.loadURDF("plane.urdf") + +#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter. +r2d2 = p.loadURDF("r2d2.urdf",[0,0,45]) +#disable linear damping +p.changeDynamics(r2d2,-1, linearDamping=0) +p.setGravity(0,0,-10) + +for i in range (3*240): + txt = "frame "+str(i) + item = p.addUserDebugText(txt, [0,1,0]) + p.stepSimulation() + #synchronize the visualizer (rendering frames for the video mp4) with stepSimulation + p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) + #print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2]) + p.removeUserDebugItem(item) + +p.disconnect() From eee7bcbc14a6546c9358ee7b930cceb1a016f6d5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 11 Jan 2020 13:16:12 -0800 Subject: [PATCH 6/6] remove debug/profile tag --- examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index c1c20cc83..5a864d318 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -355,7 +355,6 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B, // No error checking for divide by zero or instability (except with asserts) void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const { - B3_PROFILE("MatrixRmn::Solve"); assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength()); // Copy this matrix and b into an Augmented Matrix