adapt trajectory temporality depending on effector distance traveled

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -150,6 +150,13 @@ module hpp
     /// \return the value computed for the given sample and analytics
     double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
 
+    /// compute the distance between all effectors replaced between two states
+    /// does not account for contact not present in both states
+    /// \param state1 from state
+    /// \param state2 destination state
+    /// \return the value computed for the given sample and analytics
+    double getEffectorDistance(in unsigned short state1, in unsigned short state2) raises (Error);
+
     /// Generate all possible contact in a given configuration
     /// \param dofArray initial configuration of the robot
     /// \param direction desired direction of motion for the robot

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -158,6 +158,14 @@ class FullBody (object):
     # array of size 4, where each entry is the position of a corner of the contact surface
     def getEffectorPosition(self, limbName, configuration):
 		return self.client.rbprm.rbprm.getEffectorPosition(limbName,configuration)
+		
+	##compute the distance between all effectors replaced between two states
+    # does not account for contact not present in both states
+    # \param state1 from state
+    # \param state2 destination state
+    # \return the value computed for the given sample and analytics
+    def getEffectorDistance(self, state1, state2):
+		return self.client.rbprm.rbprm.getEffectorDistance(state1,state2)		
 
 	## Generates a balanced contact configuration, considering the
     #  given current configuration of the robot, and a direction of motion.
@@ -305,6 +313,7 @@ class FullBody (object):
     # \return H matrix and h column, such that H w <= h
     def getContactCone(self, stateId, friction = 0.5):
 		H_h =  array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
+		#~ print "H_h", len(H_h) 
 		# now decompose cone 
 		return H_h[:,:-1], H_h[:, -1]
 		

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

@@ -2,6 +2,7 @@ from cwc import cone_optimization
 from obj_to_constraints import ineq_from_file, rotate_inequalities
 import numpy as np
 import math
+from numpy.linalg import norm
 
 import hpp.corbaserver.rbprm.data.com_inequalities as ine
 from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
@@ -58,13 +59,21 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
 	return [np.vstack(As), np.hstack(bs)]
 		
 
-def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
+def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
 	init_com = __get_com(fullBody, states[state_id])
 	end_com = __get_com(fullBody, states[state_id+1])
 	p, N = fullBody.computeContactPoints(state_id)
 	mass = fullBody.getMass()
+	fly_time = phase_dt [1]
+	support_time = phase_dt [0]
 	t_end_phases = [0]
-	[t_end_phases.append(t_end_phases[-1]+1) for _ in range(len(p))]
+	[t_end_phases.append(t_end_phases[-1]+fly_time) for _ in range(len(p))]
+	if(len(t_end_phases) == 4):
+		t_end_phases[1] = support_time
+		t_end_phases[2] = t_end_phases[1] + fly_time
+		t_end_phases[3] = t_end_phases[2] + support_time
+		t_end_phases = [float((int)(math.ceil(el*10.))) / 10. for el in t_end_phases]
+		print "end_phases", t_end_phases
 	cones = None
 	if(computeCones):
 		cones = [fullBody.getContactCone(state_id, mu)[0]]
@@ -86,8 +95,8 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
 		
 	return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose)
 
-def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  dt=0.2, reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
-	var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, reduce_ineq, verbose, limbsCOMConstraints)
+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):
+	var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
 	p, N = fullBody.computeContactPoints(state_id)
 	from mpl_toolkits.mplot3d import Axes3D
 	import matplotlib.pyplot as plt
@@ -113,26 +122,86 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  d
 	ax.set_ylabel('Y Label')
 	ax.set_zlabel('Z Label')
 
+	plt.show()
+	
+	print "plotting speed "
+	print "end target ",  params['x_end']
+	fig = plt.figure()
+	ax = fig.add_subplot(111)
+	points = var_final['dc']
+	print "points", points
+	ys = [norm(el) for el in points]
+	xs = [i * params['dt'] for i in range(0,len(points))]
+	ax.scatter(xs, ys, c='b')
+
+
+	plt.show()
+	
+	print "plotting acceleration "
+	fig = plt.figure()
+	ax = fig.add_subplot(111)
+	points = var_final['ddc']
+	ys = [norm(el) for el in points]
+	xs = [i * params['dt'] for i in range(0,len(points))]
+	ax.scatter(xs, ys, c='b')
+
+
+	plt.show()
+	
+
 	plt.show()
 	return var_final, params
 	
-def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
+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, resultDic = {}):
 	print "callgin gen ",state_id
 	if(draw):
-		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose, limbsCOMConstraints)		
+		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)		
 	else:
-		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose, limbsCOMConstraints)
+		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
 	t = res[1]["t_init_phases"];
 	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],dt)] for index, _ in enumerate(t[:-1]) ]
+	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]))
-	#~ assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
+	resultDic[str(state_id)] = comPosPerPhase
+	return comPosPerPhase
+
+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):
+	comPosPerPhase = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints)
 	print "done. generating state trajectory ",state_id	
+	print "comePhaseLength", len(comPosPerPhase[0]),  len(comPosPerPhase[1]),  len(comPosPerPhase[2])
 	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
 	return paths_ids[-1], paths_ids[:-1]
 	
+#~ 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/hpp/corbaserver/rbprm/tools/path_to_trajectory.py

@@ -56,16 +56,18 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
 	dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
 	return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
 	
-def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_path, dt_framerate=__24fps):
+def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, dt_framerate=__24fps):
 	config_size = len(robot.getCurrentConfig())
 	res = []
 	pp = path_player
+	activeid = 0
 	for path_id in path_ids:
 		config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
 		if(config_size_path > config_size):
-			res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
+			res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[1], dt_framerate)
 		else:
-			res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
+			res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[0], dt_framerate)
+		activeid +=1
 	return res
 	
 import time

src/rbprmbuilder.impl.cc

@@ -405,6 +405,24 @@ namespace hpp {
         return cit->second[sampleId];
     }
 
+
+    double RbprmBuilder::getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error)
+    {
+        try
+        {
+            std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
+            if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
+            {
+                throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
+            }
+            return rbprm::effectorDistance(lastStatesComputed_[s1], lastStatesComputed_[s2]);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
     {
         std::vector<std::string> res;
@@ -944,6 +962,10 @@ namespace hpp {
         try
         {
             T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
+            if(positions.size() <2)
+            {
+                throw std::runtime_error("generateComPath requires at least 2 configurations to generate path");
+            }
             return addRotations(positions,q1,q2,fullBody->device_->currentConfiguration(),
                                                  fullBody->device_,problemSolver->problem());
         }
@@ -1214,7 +1236,7 @@ assert(s2 == s1 +1);
             if(!(problemSolver_->problem()->configValidations()->validate(s1Ter.configuration_, rport)
                     && problemSolver_->problem()->configValidations()->validate(s2Bis.configuration_, rport)))
             {
-                //std::cout << "could not project without collision at state " << std::string(""+s1)  << std::endl;
+                std::cout << "could not project without collision at state " << std::string(""+s1)  << std::endl;
                 throw std::runtime_error ("could not project without collision at state " + std::string(""+s1));
                 // fallback to limbRRT instead
                 //return -1; //limbRRT(s1, s2, numOptimizations);

src/rbprmbuilder.impl.hh

@@ -121,6 +121,7 @@ namespace hpp {
         virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
         virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
         virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
+        virtual double getEffectorDistance(unsigned short  state1, unsigned short  state2) throw (hpp::Error);
 
         virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
                                                 const hpp::floatSeq& direction) throw (hpp::Error);