Commit 011b4a8e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

cf previous

parent 269fe450
......@@ -8,6 +8,8 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
from numpy import array, matrix
import sample_com_vel as scv
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
......@@ -27,20 +29,22 @@ fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = ProblemSolver( fullBody )
r = Viewer (ps)
n_samples = 10000
#~ 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)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, n_samples, "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)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, n_samples, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
......@@ -51,7 +55,7 @@ rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.015; rArmy = 0.02
#disabling collision for hook
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, n_samples, "manipulability", 0.05, "_6_DOF", True)
#~ AFTER loading obstacles
larmId = '4Larm'
......@@ -61,7 +65,7 @@ lHand = 'LARM_JOINT5'
lArmOffset = [-0.045,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.015; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, n_samples, "manipulability", 0.05, "_6_DOF", True)
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
......@@ -148,6 +152,44 @@ def fill_contact_points(limbs, config, config_pinocchio):
res["N"] += N
return res
from random import randint
def pos_quat_to_pinocchio(q):
q_res = q[:]
quat_end = q[4:7]
q_res[6] = q[3]
q_res[3:6] = quat_end
return q_res
def gen_contact_candidates_one_limb(limb, config_gepetto, res, num_candidates = 10):
effectorName = limbsCOMConstraints[limb]['effector']
candidates = []
for i in range(num_candidates):
fullBody.setCurrentConfig(fullBody.getSample(limb,randint(0,n_samples - 1)))
#~ m = _getTransform(fullBody.getJointPosition(effectorName))
candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
res["candidates"][effectorName] = candidates
def find_limbs_broken(target_c, config, limbs):
def gen_contact_candidates(limbs, config_gepetto, res):
#first generate a com translation current configuration
success, dc, delta_c = scv.comTranslationAfter07s(res["q"], res["contact_points"])
#set it as new com objective for projection
fullBody.setCurrentConfig(config_gepetto)
c = array(fullBody.getCenterOfMass())
target_c = c + delta_c
state_id = fullBody.createState(config, limbs)
res["candidates"] = {}
brokenContacts = []
candidateLimbs = []
if(fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
for limb in limbs:
gen_contact_candidates_one_limb(limb, config_gepetto, res, 10)
from numpy import cos, sin, pi
def __eulerToQuat(pitch, roll, yaw):
t0 = cos(yaw * 0.5);
......@@ -264,4 +306,3 @@ gen(limbs[0], 10)
i = 0
import sample_com_vel as scv
......@@ -133,7 +133,7 @@ def gen_com_vel(q0, contacts):
def comTranslationAfter07s(q_hpp, contacts):
(success, dc0)= gen_com_vel(q_pin(q_hpp), contacts)
return success, dc0 * 0.7
return success, dc0, dc0 * 0.7
np.set_printoptions(precision=2, suppress=True);
......
......@@ -638,6 +638,7 @@ namespace hpp {
}
lastStatesComputed_.push_back(state);
lastStatesComputedTime_.push_back(std::make_pair(-1., state));
return lastStatesComputed_.size()-1;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment