Merge pull request #2074 from erwincoumans/master

fix compile issues

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

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<int> mirror_joints0, const std::vector<int> 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)
 {

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<int> mirror_joints0, const std::vector<int> mirror_joints1, Eigen::VectorXd& out_pose);
-
+	
 
 protected:
 	static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type);

examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp

@@ -3,7 +3,6 @@
 #define _USE_MATH_DEFINES
 #include <math.h>
 
-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<unsigned int*>(&seed);
-	std::default_random_engine rand_gen(int_seed);
-	std::uniform_real_distribution<double> 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<int>(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)
 {

examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h

@@ -1,11 +1,10 @@
 #pragma once
 
-#include <random>
 
 #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 <typename T>
-using tEigenArr = std::vector<T, Eigen::aligned_allocator<T> >;
-typedef tEigenArr<tVector> 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 <typename T>
 	static T SignAux(T val)
 	{

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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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<Eigen::MatrixXd, -1, -1, false> 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<const Eigen::MatrixXd> 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<const Eigen::MatrixXd> 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
 			}
 		}
 	}
-}
+}

examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp

@@ -1,140 +0,0 @@
-#include "Rand.h"
-#include <time.h>
-#include <assert.h>
-#include <algorithm>
-
-cRand::cRand()
-{
-	unsigned long int seed = static_cast<unsigned long int>(time(NULL));
-	mRandGen = std::default_random_engine(seed);
-	mRandDoubleDist = std::uniform_real_distribution<double>(0, 1);
-	mRandDoubleDistNorm = std::normal_distribution<double>(0, 1);
-	mRandIntDist = std::uniform_int_distribution<int>(std::numeric_limits<int>::min() + 1, std::numeric_limits<int>::max()); // + 1 since there is one more neg int than pos int
-	mRandUintDist = std::uniform_int_distribution<unsigned int>(std::numeric_limits<unsigned int>::min(), std::numeric_limits<unsigned int>::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<double> 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);
-}

examples/SharedMemory/plugins/stablePDPlugin/Rand.h

@@ -1,31 +0,0 @@
-#pragma once
-
-#include <random>
-
-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<double> mRandDoubleDist;
-	std::normal_distribution<double> mRandDoubleDistNorm;
-	std::uniform_int_distribution<int> mRandIntDist;
-	std::uniform_int_distribution<unsigned int> mRandUintDist;
-};

examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp

@@ -1,5 +1,6 @@
 #include "Shape.h"
 #include <assert.h>
+#include <stdio.h>
 
 bool cShape::ParseShape(const std::string& str, eShape& out_shape)
 {

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),

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):

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',

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;

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(btAlignedObjectArray<b
 		btTypedConstraint* constraint = constraints[i];
 		if (constraint->isEnabled())
 		{
-			int islandId = btGetConstraintIslandId(constraint);
+			int islandId = btGetConstraintIslandId1(constraint);
 			// if island is not sleeping,
 			if (Island* island = getIsland(islandId))
 			{

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<btJointNode> jointNodeArray;
+	btAlignedObjectArray<btJointNode1> 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)
 			{

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;
 		}
 	}