setting up profiling scripts for cwc

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -418,6 +418,11 @@ module hpp
     /// \return min and max values obtained
     floatSeq runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
 
+    /// if the preprocessor variable PROFILE is active
+    /// dump the profiling data into a logFile
+    /// \param logFile name of the file where to dump the profiling data
+    void dumpProfile(in string logFile) raises (Error);
+
   }; // interface Robot
   }; // module rbprm
   }; // module corbaserver

profile/profile.py

@@ -9,7 +9,7 @@ scenarios = ['car_hrp2']
 #~ scenarios = ['stair_bauzil_hrp2']
 n_trials = 100
 
-stats = ['balance','collision','ik']
+stats = ['balance','collision','ik','cwc_optim','com_traj']
 
 python_script_extension = "_interp.py"
 analysis = {}
@@ -67,6 +67,13 @@ def parseData(scenario):
 	data['path_time'] = []
 	data['min_path_time'] = higher
 	data['max_path_time'] = lower
+	#cwc data
+	data["optim_error_optim_fail"] = 0
+	data[  "optim_error_com_proj"] = 0
+	data[   "optim_error_unknown"] = 0
+	data[     "optim_num_success"] = 0
+	data[      "optim_num_trials"] = 0
+	data[            "num_errors"] = 0
 	for stat in stats:
 		initdata(stat, data)	
 	file = open(filename,"r+");
@@ -97,6 +104,29 @@ def parseData(scenario):
 			data['path_time'].append(time);
 			data['min_path_time'] = min(data['min_path_time'],time)
 			data['max_path_time'] = max(data['max_path_time'],time)
+		#now, analyzing optimization
+		elif not (line.find('optim_error_optim_fail') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['optim_error_optim_fail']+= val;
+		elif not (line.find('optim_error_com_proj') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['optim_error_com_proj']+= val;
+		elif not (line.find('optim_error_unknown') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['optim_error_unknown']+= val;
+		elif not (line.find('optim_num_success') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['optim_num_success']+= val;
+		elif not (line.find('optim_num_trials') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['optim_num_trials']+= val;
+		elif not (line.find('num_errors') == -1):
+			val = float(line.rstrip("\n").split()[2])
+			data['num_errors']+= val;
+		elif not (line.find('cwc_optim') == -1):
+			parseLine(1, 'cwc_optim', line, data)
+		elif not (line.find('com_traj') == -1):
+			parseLine(1, 'com_traj', line, data)
 
 
 def printOneStat(f, name, data, g_time, tTime):
@@ -141,6 +171,14 @@ def analyzeData():
 		tc = nc + c + uc 
 		f.write ("% of failed contact generation (no candidates found): " + str(nc / tc * 100)  + "%\n")
 		f.write ("% of unstable contact generation (no balanced candidates found): " + str(uc / tc * 100)  + "%\n")
+		f.write ("\n")
+		
+		f.write ("***********************%\n")
+		f.write (" optimization related data %\n")
+		f.write ("% succes rate of optimization: " + str(data["optim_num_success"] / data["optim_num_trials"] * 100)  + "%\n")
+		f.write ("% succes rate of optimization disregarding collision: " + str(data["optim_num_success"] / (data["optim_num_trials"] - data["optim_error_com_proj"]) * 100)  + "%\n")
+		f.write ("% errors due to problem infeasibility (% of errors, % over all trials): " + str(data["optim_error_optim_fail"] / data["num_errors"] * 100) + " " str(data["optim_error_optim_fail"] / data["optim_num_trials"] * 100)  + "%\n")
+		f.write ("% errors due to unknown reasons (% of errors, % over all trials): " + str(data["optim_error_unknown"] / data["num_errors"] * 100) + " " str(data["optim_error_unknown"] / data["optim_num_trials"] * 100)  + "%\n")
 		f.write ("\n \n \n")
 	f.close()
 	

profile/profile.sh

@@ -2,7 +2,7 @@
 
 gepetto-viewer-server & 
 hpp-rbprm-server &
-ipython ../script/scenarios/$1
+ipython ./scenarios/$1
 
 pkill -f  'gepetto-viewer-server'
 pkill -f  'hpp-rbprm-server'

profile/scenarios/darpa_hyq_path.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+# URDF file describing the trunk of the robot HyQ
+urdfName = 'hyq_trunk_large'
+# URDF files describing the reachable workspace of each limb of HyQ
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
+
+# Choosing a path optimizer
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ r.loadObstacleModel (packageName, "darpa", "planning")
+
+# Solve the problem
+t = ps.solve ()
+print t;
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp (1)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)

profile/scenarios/ground_crouch_hyq_bridge.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+import ground_crouch_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 20000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = fullBody.interpolate(0.1,1,10) #hole 
+#~ configs = fullBody.interpolate(0.08,1,5) # bridge
+
+r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
+i = 0;
+r (configs[i]); i=i+1; i-1
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+
+def act(i, numOptim = 0):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
+#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
+#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
+
+for i in range(4,25):
+    act(i,0)
+
+

profile/scenarios/ground_crouch_hyq_hole.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+import ground_crouch_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 20000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = fullBody.interpolate(0.1,1,10) #hole 
+#~ configs = fullBody.interpolate(0.08,1,5) # bridge
+
+r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
+i = 0;
+r (configs[i]); i=i+1; i-1
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+
+def act(i, numOptim = 0):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
+#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
+#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
+
+for i in range(4,25):
+    act(i,0)
+
+

profile/scenarios/ground_crouch_hyq_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+import ground_crouch_hyq_path as tp
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 20000
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_0 = fullBody.getCurrentConfig(); 
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+fullBody.setStartState(q_init,[])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+r(q_init)
+
+configs = fullBody.interpolate(0.1,1,10) #hole 
+#~ configs = fullBody.interpolate(0.08,1,5) # bridge
+
+r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
+i = 0;
+r (configs[i]); i=i+1; i-1
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+
+	
+limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
+						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
+						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
+
+
+
+def act(i, numOptim = 0):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
+#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
+#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
+

profile/scenarios/stair_bauzil_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_path as tp
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
+
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
+
+rKneeId = '0RKnee'
+rLeg = 'RLEG_JOINT0'
+rKnee = 'RLEG_JOINT3'
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
+rLegx = 0.05; rLegy = 0.05
+#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
+#~ 
+lKneeId = '1LKnee'
+lLeg = 'LLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'