diff --git a/script/scenarios/demos/darpa_hyq.py b/script/scenarios/demos/darpa_hyq.py index 5079b5dff549e61ba394bed38e3c32f2ccf31bd4..4e569bdb92983504f22cd352b71b6bc38452a0b6 100644 --- a/script/scenarios/demos/darpa_hyq.py +++ b/script/scenarios/demos/darpa_hyq.py @@ -43,6 +43,7 @@ cType = "_3_DOF" rLegId = 'rfleg' rLeg = 'rf_haa_joint' rfoot = 'rf_foot_joint' +#~ offset = [0.,-0.021,0.] offset = [0.,-0.021,0.] normal = [0,1,0] legx = 0.02; legy = 0.02 @@ -174,7 +175,7 @@ def genPlan(stepsize=0.06): tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") global configs start = time.clock() - configs = fullBody.interpolate(stepsize, 8, 0, filterStates = True, testReachability=False, quasiStatic=True) + configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True) end = time.clock() print "Contact plan generated in " + str(end-start) + "seconds" diff --git a/script/scenarios/demos/darpa_hyq_path.py b/script/scenarios/demos/darpa_hyq_path.py index b898b896df4a2c5dad4bdf94ae03f1743e399ce5..b06c14affc0da66b0c2c6cdc2669e8b2f2402303 100644 --- a/script/scenarios/demos/darpa_hyq_path.py +++ b/script/scenarios/demos/darpa_hyq_path.py @@ -38,7 +38,7 @@ 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, 0, 0.75]; r (q_goal) +#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal) # Choosing a path optimizer ps.addPathOptimizer("RandomShortcut") @@ -101,7 +101,7 @@ cl.problem.selectProblem("rbprm_path") rbprmBuilder2 = Robot ("toto") ps2 = ProblemSolver( rbprmBuilder2 ) cl.problem.selectProblem("default") -cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()) +cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:]) r2 = Viewer (ps2, viewerClient=r.client) r2(q_far) diff --git a/src/hpp/corbaserver/rbprm/tools/obj_to_constraints.py b/src/hpp/corbaserver/rbprm/tools/obj_to_constraints.py index 2c3601a7f729f3fb93e8d249a4aff751e93bc9a0..95db64f2f185df376acde91470f4525697d77015 100644 --- a/src/hpp/corbaserver/rbprm/tools/obj_to_constraints.py +++ b/src/hpp/corbaserver/rbprm/tools/obj_to_constraints.py @@ -39,7 +39,7 @@ def load_obj(filename) : if j!=1: face[i][j] = int(face[i][j]) - 1 F.append(face) - + fh.close() return ObjectData(V, T, N, F) def inequality(v, n): diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 794c53909cdbbd4444468670b9c6a48ef96ca09d..ddee883ed71c8125ef2fd4e4692f5952105c0504 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -912,7 +912,7 @@ namespace hpp { posConstraints.push_back(false);posConstraints.push_back(false);posConstraints.push_back(true); rotConstraints.push_back(true);rotConstraints.push_back(true);rotConstraints.push_back(true); const pinocchio::Frame effectorFrame = device->getFrameByName(limb->effector_.name()); - pinocchio::JointPtr_t effectorJoint(new pinocchio::Joint(limb->effector_.joint())); + pinocchio::JointPtr_t effectorJoint= limb->effector_.joint(); proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody()->device_, effectorJoint, effectorFrame.pinocchio().placement * globalFrame,