com trajectory is now followed accurately (not by linear interpolation btw key...

com trajectory is now followed accurately (not by linear interpolation btw key frames but by actual integration)

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -292,6 +292,19 @@ module hpp
     /// \return id of the root path computed
     short generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
 
+    /// Create a com trajectory given list of positions, velocities and accelerations
+    /// accelerations list contains one less element because it does not have an initial state.
+    /// a set of 3d key points
+    /// The path is composed of n+1 integrations
+    /// between the n positions.
+    /// The resulting path
+    /// is added to the problem solver
+    /// \param positions array of positions
+    /// \param velocities array of velocities
+    /// \param accelerations array of accelerations
+    /// \param dt time between two points
+    /// \return id of the root path computed
+    short generateComTraj(in floatSeqSeq positions, in floatSeqSeq velocities, in floatSeqSeq accelerations, in double dt) raises (Error);
 
     /// Provided a path has already been computed and interpolated, generate a continuous path
     /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -349,9 +362,9 @@ module hpp
     /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     /// \return id of the root path computed
     floatSeq comRRTFromPos(in double state1,
-                        in  floatSeqSeq rootPositions1,
-                        in  floatSeqSeq rootPositions2,
-                        in  floatSeqSeq rootPositions3,
+                        in  unsigned short comTraj1,
+                        in  unsigned short comTraj2,
+                        in  unsigned short comTraj3,
                         in unsigned short numOptimizations) raises (Error);
 
     /// Provided a path has already been computed and interpolated, generate a continuous path
@@ -366,32 +379,32 @@ module hpp
     /// \param rootPositions1 com positions to track for 3rd phase
     /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     /// \return id of the root path computed
-		floatSeq effectorRRT(in double state1,
-                        in  floatSeqSeq rootPositions1,
-                        in  floatSeqSeq rootPositions2,
-                        in  floatSeqSeq rootPositions3,
-												in unsigned short numOptimizations) raises (Error);
-
-
-		/// Provided a path has already been computed and interpolated, generate a continuous path
-		/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
-		/// Will fail if the index of the states do not exist
-		/// The path of the COM of thr robot and limbs not considered by the contact transitions between
-		/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
-		/// It must be valid in the sense of the active PathValidation.
-		/// \param state1 index of first state.
-		/// \param rootPositions1 com positions to track for 1st phase
-		/// \param rootPositions1 com positions to track for 2nd phase
-		/// \param rootPositions1 com positions to track for 3rd phase
-		/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
-		/// \return id of the root path computed
-		floatSeq effectorRRTFromPath(in double state1, in  unsigned short refPath,
-												in double path_from, in double path_to,
-												in floatSeqSeq rootPositions1,
-												in floatSeqSeq rootPositions2,
-												in floatSeqSeq rootPositions3,
-												in unsigned short numOptimizations,
-												in Names_t trackedEffectors) raises (Error);
+    floatSeq effectorRRT(in double state1,
+                        in  unsigned short comTraj1,
+                        in  unsigned short comTraj2,
+                        in  unsigned short comTraj3,
+                        in unsigned short numOptimizations) raises (Error);
+
+
+    /// Provided a path has already been computed and interpolated, generate a continuous path
+    /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
+    /// Will fail if the index of the states do not exist
+    /// The path of the COM of thr robot and limbs not considered by the contact transitions between
+    /// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
+    /// It must be valid in the sense of the active PathValidation.
+    /// \param state1 index of first state.
+    /// \param rootPositions1 com positions to track for 1st phase
+    /// \param rootPositions1 com positions to track for 2nd phase
+    /// \param rootPositions1 com positions to track for 3rd phase
+    /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+    /// \return id of the root path computed
+    floatSeq effectorRRTFromPath(in double state1, in  unsigned short refPath,
+                                    in double path_from, in double path_to,
+                                    in  unsigned short comTraj1,
+                                    in  unsigned short comTraj2,
+                                    in  unsigned short comTraj3,
+                                    in unsigned short numOptimizations,
+                                    in Names_t trackedEffectors) raises (Error);
 
     /// Project a given state into a given COM position
     /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -401,12 +414,12 @@ module hpp
     /// \return projected configuration
     floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
 
-		/// Retrieve the configuration at a given state
-		/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
-		/// Will fail if the index of the state does not exist.
-		/// \param state index of first state.
-		/// \return projected configuration
-		floatSeq getConfigAtState(in unsigned short state) raises (Error);
+    /// Retrieve the configuration at a given state
+    /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
+    /// Will fail if the index of the state does not exist.
+    /// \param state index of first state.
+    /// \return projected configuration
+    floatSeq getConfigAtState(in unsigned short state) raises (Error);
 
     /// \param limb name of the limb for which the request aplies
     /// \param state1 current state considered

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -407,6 +407,21 @@ class FullBody (object):
     #  \param quat_2 quaternion of 2nd rotation
     def generateRootPath(self, positions, quat_1, quat_2):
 		return self.client.rbprm.rbprm.generateRootPath(positions, quat_1, quat_2)
+	
+	## Create a com trajectory given list of positions, velocities and accelerations
+    # accelerations list contains one less element because it does not have an initial state.
+    # a set of 3d key points
+    # The path is composed of n+1 integrations
+    # between the n positions.
+    # The resulting path
+    # is added to the problem solver
+    # \param positions array of positions
+    # \param velocities array of velocities
+    # \param accelerations array of accelerations
+    # \param dt time between two points
+    # \return id of the root path computed
+    def generateComTraj(self, positions, velocities, accelerations, dt):
+		return self.client.rbprm.rbprm.generateComTraj(positions, velocities, accelerations, dt)
 		
 	## Create a path for the root given
     #  a set of 3d key points

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -45,9 +45,6 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
 	for i, v in limbsCOMConstraints.iteritems():
 		if not constraintsComLoaded.has_key(i):
 			constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
-		#~ print "inter", interm
-		#~ print "intermed", fullBody.isLimbInContactIntermediary(i, state)
-		#~ print "inter", fullBody.isLimbInContact(i, state)
 		contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
 		if contact:
 			ineq = constraintsComLoaded[i]
@@ -56,9 +53,7 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
 			tr[:3,3] = np.array(qEffector[0:3])
 			ineq_r = rotate_inequalities(ineq, tr)
 			As.append(ineq_r.A); bs.append(ineq_r.b);
-			#~ print 'contact', v['effector']
 			contacts.append(v['effector'])
-	#~ print 'contacts', contacts
 	return [np.vstack(As), np.hstack(bs)]
 		
 def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints):
@@ -83,13 +78,11 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
 			cones.append(fullBody.getContactCone(state_id+1, mu)[0])		
 	COMConstraints = None
 	if(not (limbsCOMConstraints == None)):
-		#~ print "retrieving COM constraints"
 		COMConstraints = [__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints)]
 		if(len(p) > 2):
 			COMConstraints.append(__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
 		if(len(p) > len(COMConstraints)):
 			COMConstraints.append(__get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
-		#~ print "num com constraints", len(COMConstraints)
 	return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
 
 
@@ -101,7 +94,6 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	
 	constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
 	#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
-	#~ constraints = ['end_reached_constraint']
 	param_constraints = []	
 	mass = fullBody.getMass()
 	
@@ -112,7 +104,6 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	for w in range(1,use_window+1):
 		waypoint = end_com[:]
 		waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
-		print "waypoint_time", waypoint_time
 		# trying not to apply constraint
 		#~ param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
 		p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints)
@@ -131,12 +122,10 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 	timeelapsed = 0		
 	if (profile):
 		start = time.clock() 
-	#~ print "init x", init_com + lastspeed.tolist()
 	var_final, params = cone_optimization(p, N, [init_com + lastspeed.tolist(), end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose,
 	constraints, param_constraints)	
 	print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
 	if (profile):
-	#~ if (True):
 		end = time.clock() 
 		timeelapsed = (end - start) * 1000
 		#~ print "solving time", timeelapsed
@@ -151,8 +140,11 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
 		else:
 			print "reached com is not good, restarting problem with 0 window"
 			return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt[:2], reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = 0)			
-	else:
+	else:		
+		if norm(np.array(var_final['c'][-1]) - np.array(__get_com(fullBody, states[state_id+1]))) > 0.00001:
+			print "error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1]
 		lastspeed = np.array([0,0,0])
+		
 	return var_final, params, timeelapsed
 
 def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = 0):
@@ -212,6 +204,20 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	plt.show()
 	return var_final, params, elapsed
 	
+def __cVarPerPhase(var, dt, t, final_state, addValue, isAcceleration = False):
+	varVals = addValue + [v.tolist() for v in final_state[var]]
+	varPerPhase = [[varVals[(int)(t_id/dt) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1])  ]
+	varPerPhase[2].append(varVals[-1])
+	if(not isAcceleration):
+		assert len(varVals) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of c or dc"
+		
+	varPerPhase[0].append(varPerPhase[1][0]) # duplicate position
+	varPerPhase[1].append(varPerPhase[2][0])
+	if(isAcceleration): #acceleration: remove first
+		varPerPhase = [v[1:] for v in varPerPhase]
+		assert len(final_state[var]) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of ddc"
+	return varPerPhase
+	
 def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
  num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
 	print "callgin gen ",state_id
@@ -223,11 +229,14 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
 	dt = res[1]["dt"];
 	final_state = res[0]
 	c0 =  res[1]["x_init"][0:3]
-	comPos = [c0] + [c.tolist() for c in final_state['c']]
-	comPosPerPhase = [[comPos[(int)(t_id/dt) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1])  ]
-	comPosPerPhase[-1].append(comPos[-1])
-	assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
-	return comPosPerPhase, res[2] #res[2] is timeelapsed
+	dc0 = res[1]["x_init"][3:7]
+	comPosPerPhase = __cVarPerPhase('c'  , dt, t, final_state, [c0])
+	comVelPerPhase = __cVarPerPhase('dc' , dt, t, final_state, [dc0])
+	comAccPerPhase = __cVarPerPhase('ddc', dt, t, final_state, [[0,0,0]], isAcceleration = True)
+	
+	#now compute com trajectorirs
+	comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
+	return comTrajIds, res[2] #res[2] is timeelapsed
 
 def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
 reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
@@ -243,8 +252,6 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
 	comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
 	reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
 	print "done. generating state trajectory ",state_id		
-	if norm(np.array(comPosPerPhase[2][-1]) - np.array(__get_com(fullBody, states[state_id+1]))) > 0.00001:
-		print "error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), comPosPerPhase[2][-1]
 	if(len(trackedEffectors) == 0):
 		paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
 	else:
@@ -254,30 +261,3 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
 	print "done. computing final trajectory to display ",state_id
 	return paths_ids[-1], paths_ids[:-1], timeElapsed
 	
-#~ from multiprocessing import Process	
-
-#~ def solve_com_RRTs(fullBody, states, state_ids, computeCones = False, mu = 1, dt =0.1, phase_dt = 1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
-	#~ results = {}
-	#~ processes = {}
-	#~ allpathsids =[[],[]]
-	#~ errorid = []
-	#~ for sid in state_ids:
-		#~ pid = str(sid)
-		#~ p = Process(target=__optim__threading_ok, args=(fullBody, states, sid, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, results))
-		#~ processes[str(sid)] = p
-		#~ p.start()
-	#~ for i,p in processes.iteritems():
-		#~ p.join()
-	#~ print results
-	#~ print "done. generating state trajectory "
-	#~ for sid in state_ids:
-		#~ comPosPerPhase = results[str(sid)]
-		#~ try:
-			#~ paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
-			#~ print "done. computing final trajectory to display ",state_id
-			#~ allpathsids[0].append(paths_ids[-1])
-			#~ allpathsids[1].append(paths_ids[:-1])
-		#~ except:
-			#~ errorid += [i]
-	#~ print "errors at states: "; errorid
-	#~ return allpathsids[0], allpathsids[1]

src/rbprmbuilder.impl.hh

@@ -19,6 +19,7 @@
 # define HPP_RBPRM_CORBA_BUILDER_IMPL_HH
 
 # include <hpp/core/problem-solver.hh>
+# include <hpp/core/path.hh>
 # include "rbprmbuilder.hh"
 # include <hpp/rbprm/rbprm-device.hh>
 # include <hpp/rbprm/rbprm-fullbody.hh>
@@ -152,29 +153,40 @@ namespace hpp {
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
+        virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,
+                                             const hpp::floatSeqSeq& accelerations, const double dt) throw (hpp::Error);
         virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
                                       const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
         virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
         virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
         virtual CORBA::Short configToPath(const hpp::floatSeqSeq& configs) throw (hpp::Error);
         virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+
+        typedef core::PathPtr_t (*t_rrt)
+            (RbPrmFullBodyPtr_t, core::ProblemPtr_t, const core::PathPtr_t,
+             const  State &, const State &, const  std::size_t, const bool);
+
+        hpp::floatSeq* rrt(t_rrt functor ,double state1,
+                           unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
+                           unsigned short numOptimizations) throw (hpp::Error);
+
         virtual hpp::floatSeq* comRRTFromPos(double state1,
-                                           const hpp::floatSeqSeq& rootPositions1,
-                                           const hpp::floatSeqSeq& rootPositions2,
-                                           const hpp::floatSeqSeq& rootPositions3,
+                                           unsigned short comTraj1,
+                                           unsigned short comTraj2,
+                                           unsigned short comTraj3,
                                            unsigned short numOptimizations) throw (hpp::Error);
         virtual hpp::floatSeq* effectorRRT(double state1,
-                                           const hpp::floatSeqSeq& rootPositions1,
-                                           const hpp::floatSeqSeq& rootPositions2,
-                                           const hpp::floatSeqSeq& rootPositions3,
+                                           unsigned short comTraj1,
+                                           unsigned short comTraj2,
+                                           unsigned short comTraj3,
                                            unsigned short numOptimizations) throw (hpp::Error);
         virtual hpp::floatSeq* effectorRRTFromPath(double state1,
                                            unsigned short path,
                                            double path_from,
                                            double path_to,
-                                           const hpp::floatSeqSeq& rootPositions1,
-                                           const hpp::floatSeqSeq& rootPositions2,
-                                           const hpp::floatSeqSeq& rootPositions3,
+                                           unsigned short comTraj1,
+                                           unsigned short comTraj2,
+                                           unsigned short comTraj3,
                                            unsigned short numOptimizations,
                                            const hpp::Names_t& trackedEffectors) throw (hpp::Error);
         virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);