Commit 3cd06448 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

generate contact from config

parent 6dccc4b4
......@@ -6,6 +6,7 @@ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
from numpy import array
packageName = "hrp2_14_description"
......@@ -48,11 +49,14 @@ 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)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "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'},
#~ lLegId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand},
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}
#~ AFTER loading obstacles
larmId = '4Larm'
......@@ -61,24 +65,48 @@ 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, "manipulability", 0.05, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
import quaternion as quat
def _getTransform(qEffector):
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix() #compute rotation matrix local -> world
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
rm=np.zeros((4,4))
for k in range(0,3):
for l in range(0,3):
rm[k,l] = rot[k,l]
for m in range(0,3):
rm[m,3] = qEffector[m]
rm[3,3] = 1
return rm
def draw_cp(cid, limb, config):
global r
posetc = fullBody.getEffectorPosition(limb, config)
print "pos ", posetc[0]
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(config, limb)
fullBody.setCurrentConfig(config)
effectorName = limbsCOMConstraints[limb]['effector']
limbId = limb
m = _getTransform(fullBody.getJointPosition(effectorName))
scene = "qds"+limb+str(cid)
r.client.gui.createScene(scene)
for i in range(4):
pos = posetc[2*i]
#~ pos = posetc[2*i]
print array(P[i]+[1])
print m.dot(array(P[i]+[1]))
pos = m.dot(array(P[i]+[1]))[:3]
print "pos", pos
r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos.tolist()+[1,0,0,0])
r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0)
......@@ -90,12 +118,19 @@ def fill_contact_points(limbs, config, config_pinocchio):
res["P"] = []
res["N"] = []
for limb in limbs:
posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][limb] = {}
res["contact_points"][limb]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][limb]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
effector = limbsCOMConstraints[limb]['effector']
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(q, limb)
#~ posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][effector] = {}
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][effector]["P"] = P
#~ res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += P
#~ res["contact_points"][effector]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["contact_points"][effector]["N"] = N
#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += N
return res
q_0 = fullBody.getCurrentConfig();
......
......@@ -314,9 +314,9 @@ class FullBody (object):
# \param q configuration of the robot
# \param limbName ids of the limb in contact
# \return list 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
def computeContactForConfig(q, limbName):
rawdata = self.computeContactPointsForLimb(stateId, limb)
return [[rawdata[i:i+3]] for i in range(0, len(rawdata), 6)], [[rawdata[i+3:i+6]] for i in range(0, len(rawdata), 6)]
def computeContactForConfig(self, q, limbName):
rawdata = self.client.rbprm.rbprm.computeContactForConfig(q, limbName)
return [rawdata[i:i+3] for i in range(0, len(rawdata), 6)], [rawdata[i+3:i+6] for i in range(0, len(rawdata), 6)]
## Provided a discrete contact sequence has already been computed, computes
# all the contact positions and normals for a given state, the next one, and the intermediate between them.
......
......@@ -640,8 +640,9 @@ namespace hpp {
core::BasicConfigurationShooterPtr_t shooter = core::BasicConfigurationShooter::create(fullBody_->device_);
for(int i =0; i< 1000; ++i)
{
core::DevicePtr_t device = fullBody_->device_->clone();
std::vector<std::string> names = stringConversion(contactLimbs);
core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(fullBody_->device_,"proj", 1e-4, 1000);
core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(device,"proj", 1e-4, 40);
//hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), proj);
for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
{
......@@ -900,7 +901,7 @@ namespace hpp {
SetPositionAndNormal(state,fullBody_, configuration, limbs);
const std::vector<fcl::Vec3f>& positions = computeRectangleContact(fullBody_,state,limb);
_CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
_CORBA_ULong size = (_CORBA_ULong) (positions.size () * 3);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(size);
for(std::size_t h = 0; h<positions.size(); ++h)
......@@ -908,7 +909,7 @@ namespace hpp {
for(std::size_t k =0; k<3; ++k)
{
model::size_type j (h*3 + k);
dofArray[j] = positions[h][k];
(*dofArray)[(_CORBA_ULong)j] = positions[h][k];
}
}
return dofArray;
......
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