diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 68e3492b3..9e0652adf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -71,7 +71,7 @@ #ifdef STATIC_LINK_SPD_PLUGIN #include "plugins/stablePDPlugin/BulletConversion.h" #include "plugins/stablePDPlugin/RBDModel.h" -#include "plugins/stablePDPlugin/RBDutil.h" +#include "plugins/stablePDPlugin/RBDUtil.h" #endif #ifdef STATIC_LINK_VR_PLUGIN diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp index 1b326f02d..2aab56904 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp @@ -1771,64 +1771,6 @@ void cKinTree::NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::Vec } } -void cKinTree::MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose) -{ - // mirrors along xy plane - assert(mirror_joints0.size() == mirror_joints1.size()); - int num_joints = cKinTree::GetNumJoints(joint_mat); - - tQuaternion root_rot = cKinTree::GetRootRot(joint_mat, out_pose); - tVector root_pos = cKinTree::GetRootPos(joint_mat, out_pose); - root_pos[2] *= -1; - root_rot = cMathUtil::MirrorQuaternion(root_rot, cMathUtil::eAxisZ); - - cKinTree::SetRootRot(joint_mat, root_rot, out_pose); - cKinTree::SetRootPos(joint_mat, root_pos, out_pose); - - for (int j = 0; j < num_joints; ++j) - { - cKinTree::eJointType joint_type = cKinTree::GetJointType(joint_mat, j); - bool is_root = cKinTree::IsRoot(joint_mat, j); - if (!is_root) - { - int param_offset = cKinTree::GetParamOffset(joint_mat, j); - int param_size = cKinTree::GetParamSize(joint_mat, j); - auto joint_params = out_pose.segment(param_offset, param_size); - - if (joint_type == cKinTree::eJointTypeRevolute - || joint_type == cKinTree::eJointTypeFixed) - { - } - else if (joint_type == cKinTree::eJointTypeSpherical) - { - tQuaternion quat = cMathUtil::VecToQuat(joint_params); - quat = cMathUtil::MirrorQuaternion(quat, cMathUtil::eAxisZ); - joint_params = cMathUtil::QuatToVec(quat); - } - else - { - assert(false); // unsupported joint type - } - } - } - - for (size_t i = 0; i < mirror_joints0.size(); ++i) - { - int id0 = mirror_joints0[i]; - int id1 = mirror_joints1[i]; - - int offset0 = cKinTree::GetParamOffset(joint_mat, id0); - int size0 = cKinTree::GetParamSize(joint_mat, id0); - int offset1 = cKinTree::GetParamOffset(joint_mat, id1); - int size1 = cKinTree::GetParamSize(joint_mat, id1); - - Eigen::VectorXd params0 = out_pose.segment(offset0, size0); - Eigen::VectorXd params1 = out_pose.segment(offset1, size1); - out_pose.segment(offset0, size0) = params1; - out_pose.segment(offset1, size1) = params0; - } -} - tMatrix cKinTree::ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h index 6c2fcc31e..a1c459f43 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h @@ -238,8 +238,7 @@ public: static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose); static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel); - static void MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector mirror_joints0, const std::vector mirror_joints1, Eigen::VectorXd& out_pose); - + protected: static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type); diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp index dcdc07b60..6fcd67e16 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp @@ -3,7 +3,6 @@ #define _USE_MATH_DEFINES #include -cRand cMathUtil::gRand = cRand(); int cMathUtil::Clamp(int val, int min, int max) { @@ -45,69 +44,6 @@ double cMathUtil::NormalizeAngle(double theta) return norm_theta; } -double cMathUtil::RandDouble() -{ - return RandDouble(0, 1); -} - -double cMathUtil::RandDouble(double min, double max) -{ - return gRand.RandDouble(min, max); -} - -double cMathUtil::RandDoubleNorm(double mean, double stdev) -{ - return gRand.RandDoubleNorm(mean, stdev); -} - -double cMathUtil::RandDoubleExp(double lambda) -{ - return gRand.RandDoubleExp(lambda); -} - -double cMathUtil::RandDoubleSeed(double seed) -{ - unsigned int int_seed = *reinterpret_cast(&seed); - std::default_random_engine rand_gen(int_seed); - std::uniform_real_distribution dist; - return dist(rand_gen); -} - -int cMathUtil::RandInt() -{ - return gRand.RandInt(); -} - -int cMathUtil::RandInt(int min, int max) -{ - return gRand.RandInt(min, max); -} - -int cMathUtil::RandUint() -{ - return gRand.RandUint(); -} - -int cMathUtil::RandUint(unsigned int min, unsigned int max) -{ - return gRand.RandUint(min, max); -} - -int cMathUtil::RandIntExclude(int min, int max, int exc) -{ - return gRand.RandIntExclude(min, max, exc); -} - -void cMathUtil::SeedRand(unsigned long int seed) -{ - gRand.Seed(seed); - srand(gRand.RandInt()); -} - -int cMathUtil::RandSign() -{ - return gRand.RandSign(); -} double cMathUtil::SmoothStep(double t) { @@ -115,10 +51,6 @@ double cMathUtil::SmoothStep(double t) return val; } -bool cMathUtil::FlipCoin(double p) -{ - return gRand.FlipCoin(p); -} tMatrix cMathUtil::TranslateMat(const tVector& trans) { @@ -665,26 +597,6 @@ double cMathUtil::Sigmoid(double x, double gamma, double bias) return val; } -int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs) -{ - assert(std::abs(probs.sum() - 1) < 0.00001); - double rand = RandDouble(); - - int rand_idx = gInvalidIdx; - int num_probs = static_cast(probs.size()); - for (int i = 0; i < num_probs; ++i) - { - double curr_prob = probs[i]; - rand -= curr_prob; - - if (rand <= 0) - { - rand_idx = i; - break; - } - } - return rand_idx; -} tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c) { @@ -791,32 +703,7 @@ bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size return new_action; } -tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max) -{ - tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]), - RandDouble(bound_min[1], bound_max[1]), - RandDouble(bound_min[2], bound_max[2]), 0); - return pt; -} -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max) -{ - return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min)); -} - -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus) -{ - double t = RandDouble(0, 1); - tVector size = bound_max - bound_min; - tVector new_min = focus + (t * 0.5) * size; - tVector new_max = focus - (t * 0.5) * size; - tVector offset = (bound_min - new_min).cwiseMax(0); - offset += (bound_max - new_max).cwiseMin(0); - new_min += offset; - new_max += offset; - - return SampleRandPt(new_min, new_max); -} void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h index 4e7413ed5..8d3fe27c7 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h @@ -1,11 +1,10 @@ #pragma once -#include #include "Eigen/Dense" #include "Eigen/StdVector" #include "Eigen/Geometry" -#include "Rand.h" + #define _USE_MATH_DEFINES #include "math.h" @@ -18,9 +17,6 @@ typedef Eigen::Matrix4d tMatrix; typedef Eigen::Matrix3d tMatrix3; typedef Eigen::Quaterniond tQuaternion; -template -using tEigenArr = std::vector >; -typedef tEigenArr tVectorArr; const double gRadiansToDegrees = 57.2957795; const double gDegreesToRadians = 1.0 / gRadiansToDegrees; @@ -47,20 +43,6 @@ public: static double NormalizeAngle(double theta); - // rand number - static double RandDouble(); - static double RandDouble(double min, double max); - static double RandDoubleNorm(double mean, double stdev); - static double RandDoubleExp(double lambda); - static double RandDoubleSeed(double seed); - static int RandInt(); - static int RandInt(int min, int max); - static int RandUint(); - static int RandUint(unsigned int min, unsigned int max); - static int RandIntExclude(int min, int max, int exc); - static void SeedRand(unsigned long int seed); - static int RandSign(); - static bool FlipCoin(double p = 0.5); static double SmoothStep(double t); // matrices @@ -117,7 +99,6 @@ public: static double Sigmoid(double x); static double Sigmoid(double x, double gamma, double bias); - static int SampleDiscreteProb(const Eigen::VectorXd& probs); static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c); static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max); @@ -134,19 +115,14 @@ public: // check if curr_val and curr_val - delta belong to different intervals static bool CheckNextInterval(double delta, double curr_val, double int_size); - static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max); - // samples a bound within the given bounds with a benter towards the focus pt - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max); - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus); - + static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist); static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir); static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x); private: - static cRand gRand; - + template static T SignAux(T val) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp index 64a5820a8..6e885dea4 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp @@ -31,7 +31,8 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, cSpAlg::tSpTrans parent_child_trans = model.GetSpParentChildTrans(j); cSpAlg::tSpTrans world_child_trans = model.GetSpWorldJointTrans(j); - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); + Eigen::VectorXd q; Eigen::VectorXd dq; Eigen::VectorXd ddq; @@ -84,7 +85,7 @@ void cRBDUtil::SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, if (cKinTree::IsValidBody(body_defs, j)) { cSpAlg::tSpVec curr_f = fs.row(j); - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); Eigen::VectorXd curr_tau = S.transpose() * curr_f; cKinTree::SetJointParams(joint_mat, j, curr_tau, out_tau); @@ -156,7 +157,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf { if (cKinTree::IsValidBody(body_defs, j)) { - const auto curr_I = Is.block(j * svs, 0, svs, svs); + Eigen::Block< Eigen::MatrixXd, -1, -1, false> curr_I = Is.block(j * svs, 0, svs, svs); int parent_id = cKinTree::GetParent(joint_mat, j); if (cKinTree::HasParent(joint_mat, j)) { @@ -166,7 +167,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf Is.block(parent_id * svs, 0, svs, svs) += child_parent_mat * curr_I * parent_child_mat; } - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); int dim = cKinTree::GetParamSize(joint_mat, j); if (dim > 0) { @@ -186,7 +187,7 @@ void cRBDUtil::BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buf if (curr_dim > 0) { - const auto curr_S = model.GetJointSubspace(curr_id); + const Eigen::Block curr_S = model.GetJointSubspace(curr_id); H.block(offset, curr_offset, dim, curr_dim) = F.transpose() * curr_S; H.block(curr_offset, offset, curr_dim, dim) = H.block(offset, curr_offset, dim, curr_dim).transpose(); } @@ -261,7 +262,7 @@ Eigen::MatrixXd cRBDUtil::MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, c int size = cKinTree::GetParamSize(joint_mat, curr_id); Eigen::VectorXd curr_q; cKinTree::GetJointParams(joint_mat, q, curr_id, curr_q); - const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); joint_vel += curr_J * curr_q; curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -299,7 +300,7 @@ Eigen::MatrixXd cRBDUtil::ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat { int offset = cKinTree::GetParamOffset(joint_mat, curr_id); int size = cKinTree::GetParamSize(joint_mat, curr_id); - const auto curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_J = J.block(0, offset, cSpAlg::gSpVecSize, size); J_end_eff.block(0, offset, cSpAlg::gSpVecSize, size) = curr_J; curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -353,7 +354,7 @@ void cRBDUtil::BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J int size = cKinTree::GetParamSize(joint_mat, curr_id); const Eigen::MatrixXd& J_block = J.block(0, offset, cSpAlg::gSpVecSize, size); - auto J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size); + Eigen::Block J_com_block = out_J.block(0, offset, cSpAlg::gSpVecSize, size); J_com_block += mass_frac * cSpAlg::ApplyTransM(body_pos_trans, J_block); curr_id = cKinTree::GetParent(joint_mat, curr_id); @@ -456,7 +457,7 @@ cSpAlg::tSpVec cRBDUtil::CalcVelProdAcc(const cRBDModel& model, const Eigen::Mat Eigen::VectorXd dq; cKinTree::GetJointParams(joint_mat, pose, curr_id, q); cKinTree::GetJointParams(joint_mat, vel, curr_id, dq); - const auto curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size); + const Eigen::Block curr_Jd = Jd.block(0, offset, cSpAlg::gSpVecSize, size); cSpAlg::tSpVec cj = BuildCj(joint_mat, q, dq, curr_id); if (cj.squaredNorm() > 0) @@ -1035,7 +1036,7 @@ void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_f if (cKinTree::IsValidBody(body_defs, j)) { cSpAlg::tSpVec curr_f = fs.row(j); - const auto S = model.GetJointSubspace(j); + const Eigen::Block S = model.GetJointSubspace(j); Eigen::VectorXd curr_tau = S.transpose() * curr_f; cKinTree::SetJointParams(joint_mat, j, curr_tau, out_g_force); @@ -1047,4 +1048,4 @@ void cRBDUtil::CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_f } } } -} \ No newline at end of file +} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp deleted file mode 100644 index 599d2d0b7..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "Rand.h" -#include -#include -#include - -cRand::cRand() -{ - unsigned long int seed = static_cast(time(NULL)); - mRandGen = std::default_random_engine(seed); - mRandDoubleDist = std::uniform_real_distribution(0, 1); - mRandDoubleDistNorm = std::normal_distribution(0, 1); - mRandIntDist = std::uniform_int_distribution(std::numeric_limits::min() + 1, std::numeric_limits::max()); // + 1 since there is one more neg int than pos int - mRandUintDist = std::uniform_int_distribution(std::numeric_limits::min(), std::numeric_limits::max()); -} - -cRand::cRand(unsigned long int seed) -{ - Seed(seed); -} - -cRand::~cRand() -{ -} - -double cRand::RandDouble() -{ - return mRandDoubleDist(mRandGen); -} - -double cRand::RandDouble(double min, double max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max] - double rand_double = mRandDoubleDist(mRandGen); - rand_double = min + (rand_double * (max - min)); - return rand_double; -} - -double cRand::RandDoubleExp(double lambda) -{ - std::exponential_distribution dist(lambda); - double rand_double = dist(mRandGen); - return rand_double; -} - -double cRand::RandDoubleNorm(double mean, double stdev) -{ - double rand_double = mRandDoubleDistNorm(mRandGen); - rand_double = mean + stdev * rand_double; - return rand_double; -} - -int cRand::RandInt() -{ - return mRandIntDist(mRandGen); -} - -int cRand::RandInt(int min, int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = std::abs(RandInt()); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandUint() -{ - return mRandUintDist(mRandGen); -} - -int cRand::RandUint(unsigned int min, unsigned int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = RandUint(); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandIntExclude(int min, int max, int exc) -{ - int rand_int = 0; - if (exc < min || exc >= max) - { - rand_int = RandInt(min, max); - } - else - { - int new_max = max - 1; - if (new_max <= min) - { - rand_int = min; - } - else - { - rand_int = RandInt(min, new_max); - if (rand_int >= exc) - { - ++rand_int; - } - } - } - return rand_int; -} - -void cRand::Seed(unsigned long int seed) -{ - mRandGen.seed(seed); - mRandDoubleDist.reset(); - mRandDoubleDistNorm.reset(); - mRandIntDist.reset(); - mRandUintDist.reset(); -} - -int cRand::RandSign() -{ - return FlipCoin() ? -1 : 1; -} - -bool cRand::FlipCoin(double p) -{ - return (RandDouble(0, 1) < p); -} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h b/examples/SharedMemory/plugins/stablePDPlugin/Rand.h deleted file mode 100644 index c7dd543bf..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include - -class cRand -{ -public: - cRand(); - cRand(unsigned long int seed); - virtual ~cRand(); - - virtual double RandDouble(); - virtual double RandDouble(double min, double max); - virtual double RandDoubleExp(double lambda); - virtual double RandDoubleNorm(double mean, double stdev); - virtual int RandInt(); - virtual int RandInt(int min, int max); - virtual int RandUint(); - virtual int RandUint(unsigned int min, unsigned int max); - virtual int RandIntExclude(int min, int max, int exc); - virtual void Seed(unsigned long int seed); - virtual int RandSign(); - virtual bool FlipCoin(double p = 0.5); - -private: - std::default_random_engine mRandGen; - std::uniform_real_distribution mRandDoubleDist; - std::normal_distribution mRandDoubleDistNorm; - std::uniform_int_distribution mRandIntDist; - std::uniform_int_distribution mRandUintDist; -}; \ No newline at end of file diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp index 67fae6ca0..a8507257b 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp @@ -1,5 +1,6 @@ #include "Shape.h" #include +#include bool cShape::ParseShape(const std::string& str, eShape& out_shape) { diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 64d385cbe..6fb538089 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -92,8 +92,8 @@ struct TinyRendererVisualShapeConverterInternalData SimpleCamera m_camera; TinyRendererVisualShapeConverterInternalData() - : m_upAxis(2), - m_uidGenerator(1), + : m_uidGenerator(1), + m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB), diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py index b2ebf0994..1c7dbcc3a 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py @@ -438,18 +438,18 @@ class Humanoid(object): bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot) bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot) - bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50) - bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150) - bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90) - bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100) - bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60) - bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200) - bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150) - bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90) - bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100) - bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60) + bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=[50]) + bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=[150]) + bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=[90]) + bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=[100]) + bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=[60]) + bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=[200]) + bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=[150]) + bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=[90]) + bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=[100]) + bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=[60]) #debug space #if (False): diff --git a/setup.py b/setup.py index 98efabdbf..f115d8037 100644 --- a/setup.py +++ b/setup.py @@ -47,6 +47,8 @@ CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT ' CXX_FLAGS += '-DB3_ENABLE_FILEIO_PLUGIN ' CXX_FLAGS += '-DB3_USE_ZIPFILE_FILEIO ' CXX_FLAGS += '-DBT_THREADSAFE=1 ' +CXX_FLAGS += '-DSTATIC_LINK_SPD_PLUGIN ' + EGL_CXX_FLAGS = '' @@ -98,6 +100,13 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/PosixSharedMemory.cpp"]\ +["examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp"]\ +["examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp"]\ ++["examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp"]\ +["examples/SharedMemory/PhysicsClientUDP.cpp"]\ +["examples/SharedMemory/PhysicsClientUDP_C_API.cpp"]\ +["examples/SharedMemory/PhysicsClientTCP.cpp"]\ @@ -585,7 +594,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.0', + version='2.4.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 65b669e1c..9694f4ddb 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -27,7 +27,7 @@ btConvexPolyhedron::~btConvexPolyhedron() { } -inline bool IsAlmostZero(const btVector3& v) +inline bool IsAlmostZero1(const btVector3& v) { if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; @@ -122,8 +122,8 @@ void btConvexPolyhedron::initialize() for (int p = 0; p < m_uniqueEdges.size(); p++) { - if (IsAlmostZero(m_uniqueEdges[p] - edge) || - IsAlmostZero(m_uniqueEdges[p] + edge)) + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || + IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; break; diff --git a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp index 17287aa82..5353fe009 100644 --- a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -65,7 +65,7 @@ inline int getIslandId(const btPersistentManifold* lhs) return islandId; } -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs) { const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); @@ -452,7 +452,7 @@ void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArrayisEnabled()) { - int islandId = btGetConstraintIslandId(constraint); + int islandId = btGetConstraintIslandId1(constraint); // if island is not sleeping, if (Island* island = getIsland(islandId)) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp index 338e8af0a..f2186a93e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp @@ -22,9 +22,9 @@ subject to the following restrictions: #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS -static bool interleaveContactAndFriction = false; +static bool interleaveContactAndFriction1 = false; -struct btJointNode +struct btJointNode1 { int jointIndex; // pointer to enclosing dxJoint object int otherBodyIndex; // *other* body this joint is connected to @@ -241,7 +241,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal) { - int numContactRows = interleaveContactAndFriction ? 3 : 1; + int numContactRows = interleaveContactAndFriction1 ? 3 : 1; int numConstraintRows = m_allConstraintPtrArray.size(); @@ -301,7 +301,7 @@ void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSol BT_PROFILE("bodyJointNodeArray.resize"); bodyJointNodeArray.resize(numBodies, -1); } - btAlignedObjectArray jointNodeArray; + btAlignedObjectArray jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); @@ -729,7 +729,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( int firstContactConstraintOffset = dindex; // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction) + if (interleaveContactAndFriction1) { for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { @@ -785,7 +785,7 @@ btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( firstContactConstraintOffset = dindex; // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction) + if (interleaveContactAndFriction1) { for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) { diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 4b1ba3ba4..ec9a56229 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -486,7 +486,10 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx transformZ(q(body.m_q_index + 2)); body.m_body_T_parent = T * body.m_body_T_parent_ref; - body.m_parent_pos_parent_body.setValue(0,0,0); + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; if (type >= POSITION_VELOCITY) @@ -850,7 +853,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool transformZ(q(body.m_q_index + 2)); body.m_body_T_parent = T * body.m_body_T_parent_ref; - body.m_parent_pos_parent_body.setValue(0, 0, 0); + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } }