adding Jump to inverted pendulum controller, (wip)

so far, the robot fails when it is on the air, because the controller does not know what do do.
the goal is to sendthe arm to the rest position, and the wheel to zero angular velocity.

so far this fails.

newton-4.00/applications/ndSandbox/demos/ndUnicycle.cpp

@@ -49,6 +49,7 @@ namespace ndUnicycle
 		m_topBoxOmega,
 		m_jointAngle,
 		m_wheelOmega,
+		m_isOnAir,
 		m_stateSize
 	};
 
@@ -202,6 +203,12 @@ namespace ndUnicycle
 				#endif
 
 				m_model->ApplyActions(actions);
+
+				ndFloat32 applyJumpImpule = ndRand();
+				if (applyJumpImpule > 0.999f)
+				{
+					//m_model->ApplyRandomJump();
+				}
 			}
 
 			void GetObservation(ndBrainFloat* const observation)
@@ -323,16 +330,30 @@ namespace ndUnicycle
 			if (HasSupportContact())
 			{
 				//ignore wheel omega when robot is landed
+				state[m_isOnAir] = ndReal(0.0f);
 				state[m_wheelOmega] = ndReal(0.0f);
+
 			}
 			else
 			{
-				ndAssert(0);
 				ndVector wheelOmega (m_wheel->GetOmega());
+				state[m_isOnAir] = ndReal(1.0f);
 				state[m_wheelOmega] = wheelOmega.m_z;
 			}
 		}
 
+		void ApplyRandomJump() const
+		{
+			if (HasSupportContact())
+			{
+				ndBodyDynamic* const boxBody = GetRoot()->m_body->GetAsBodyDynamic();
+
+				ndVector upVector(0.0f, 1.0f, 0.0f, 0.0f);
+				ndVector impulse(upVector.Scale(boxBody->GetMassMatrix().m_w * 2.0f));
+				boxBody->ApplyImpulsePair(impulse, ndVector::m_zero, m_timestep);
+			}
+		}
+
 		void ApplyActions(ndBrainFloat* const actions) const
 		{
 			ndFloat32 legAngle = ndFloat32(actions[m_softLegControl]) * ND_MAX_LEG_ANGLE_STEP + m_legJoint->GetAngle();
@@ -367,7 +388,7 @@ namespace ndUnicycle
 			}
 			else
 			{
-				ndAssert(0);
+				//ndTrace((" calculate jump rweard \n"));
 				return 0.0f;
 			}
 		}

newton-4.00/sdk/dBrain/ndBrainAgentContinueVPG_Trainer.h

@@ -471,7 +471,7 @@ void ndBrainAgentContinueVPG_Trainer<statesDim, actionDim>::OptimizeStep()
 		m_trajectory.SetCount(0);
 	}
 
-	bool isTeminal = IsTerminal() || (m_trajectory.GetCount() == (m_extraTrajectorySteps + m_maxTrajectorySteps));
+	bool isTeminal = IsTerminal() || (m_trajectory.GetCount() >= (m_extraTrajectorySteps + m_maxTrajectorySteps));
 	if (isTeminal)
 	{
 		SaveTrajectory();

newton-4.00/sdk/dBrain/ndBrainAgentDiscreteVPG_Trainer.h

@@ -477,7 +477,7 @@ void ndBrainAgentDiscreteVPG_Trainer<statesDim, actionDim>::OptimizeStep()
 		m_trajectory.SetCount(0);
 	}
 
-	bool isTeminal = IsTerminal() || (m_trajectory.GetCount() == (m_extraTrajectorySteps + m_maxTrajectorySteps));
+	bool isTeminal = IsTerminal() || (m_trajectory.GetCount() >= (m_extraTrajectorySteps + m_maxTrajectorySteps));
 	if (isTeminal)
 	{
 		SaveTrajectory();