def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
self.q_init[:3] = [0, 0, 0.465]
self.q_init[3:7] = [0, 0, 0, 1]
self.q_init[-6] = self.v_init
random.seed()
alpha = random.uniform(0., 2. * np.pi)
#alpha = 4.
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[:3] = [
self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465
]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
quat = Quaternion.FromTwoVectors(vx, v_goal)
self.q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
self.q_goal[-6] = self.v_goal * np.sin(alpha)
self.q_goal[-5] = -self.v_goal * np.cos(alpha)
self.v(self.q_init)
print("initial root position : ", self.q_init)
print("final root position : ", self.q_goal)
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.alpha = alpha
# write problem in files :
with open(self.status_filename, "w") as f:
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
def set_transform(origin, a, b):
from pinocchio import rpy, Quaternion
length = np.linalg.norm(b - a)
z = (b - a) / length
R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix()
origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)])
origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def createPhaseFromRBPRMState(fb, stateId, t_init=-1):
"""
Create a multicontact_api ContactPhase object from an rbprm state
:param fb: an instance of rbprm.FullBody
:param stateId: the Id of the state
:param t_init: the initial time of the contact phase
:return: a multicontact_api.ContactPhase with the same contacts as the rbprm state
"""
q = fb.getConfigAtState(stateId)
limbs_contact = fb.getAllLimbsInContact(stateId)
cp = createPhaseFromConfig(fb, q, limbs_contact, t_init)
if fb.cType == "_3_DOF":
# update the contact normal from the data in fullbody
for limbId in limbs_contact:
eeName = fb.dict_limb_joint[limbId]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(
stateId, False, limbId)
normal = np.array(n)
print("New normal : ", normal)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]),
normal)
placement = cp.contactPatch(eeName).placement
placement.rotation = quat.matrix()
cp.contactPatch(eeName).placement = placement
print("New placement : ", normal)
return cp
def rotationFromNormal(n):
"""
return a rotation matrix corresponding to the given contact normal
:param n: the normal of the surface (as a numpy array)
:return: a rotation matrix
"""
z_up = np.array([0., 0., 1.])
q = Quaternion.FromTwoVectors(z_up, n)
return q.matrix()
def contactPlacementFromRBPRMState(fb, stateId, eeName):
# get limbName from effector name :
fb.setCurrentConfig(fb.getConfigAtState(stateId))
placement = SE3FromConfig(fb.getJointPosition(eeName))
if fb.cType == "_3_DOF":
limbId = list(fb.dict_limb_joint.keys())[list(fb.dict_limb_joint.values()).index(eeName)]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId)
normal = np.array(n)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal)
placement.rotation = quat.matrix()
return placement
def createPhaseFromRBPRMState(fb, stateId, t_init = -1):
q = fb.getConfigAtState(stateId)
limbs_contact = fb.getAllLimbsInContact(stateId)
cp = createPhaseFromConfig(fb, q, limbs_contact, t_init)
if fb.cType == "_3_DOF":
# update the contact normal from the data in fullbody
for limbId in limbs_contact:
eeName = fb.dict_limb_joint[limbId]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId)
normal = np.array(n)
print("New normal : ", normal)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal)
placement = cp.contactPatch(eeName).placement
placement.rotation = quat.matrix()
cp.contactPatch(eeName).placement = placement
print("New placement : ", normal)
return cp
def set_random_configs(self):
"""
Generate random init and goal position.
these position will be on the flat part of the environment,
with an orientation such that they can be connected by a straight line,
and an angle between +- 25 degree from the x axis
"""
# select randomly the initial and final plateform, they cannot be the same
# end plateform is always after the init plateform on the x axis
init_plateform_id = random.randint(0, 3)
end_plateform_id = random.randint(init_plateform_id + 1, 4)
#if end_plateform_id >= init_plateform_id:
# end_plateform_id+=1
# set x position from the plateform choosen :
x_init = 0.16 + 0.925 * init_plateform_id
x_goal = 0.16 + 0.925 * end_plateform_id
# uniformly sample y position
y_init = random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1])
self.q_init[:3] = [x_init, y_init, self.Z_VALUE]
# y_goal must be random inside Y_BOUNDS but such that the line between q_init and q_goal is between +- MAX_ANGLE radian from the x axis
y_bound_goal = self.Y_BOUNDS[::]
y_angle_max = math.sin(self.MAX_ANGLE) * abs(x_init - x_goal) + y_init
y_angle_min = math.sin(-self.MAX_ANGLE) * abs(x_init - x_goal) + y_init
y_bound_goal[0] = max(y_angle_min, y_bound_goal[0])
y_bound_goal[1] = min(y_angle_max, y_bound_goal[1])
y_goal = random.uniform(y_bound_goal[0], y_bound_goal[1])
# compute the orientation such that q_init face q_goal :
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_init = np.matrix([x_goal - x_init, y_goal - y_init, 0]).T
quat = Quaternion.FromTwoVectors(vx, v_init)
self.q_init[3:7] = quat.coeffs().tolist()
self.q_goal = self.q_init[::]
self.q_goal[:2] = [x_goal, y_goal]
print("q_init= " + str(self.q_init))
print("q_goal= " + str(self.q_goal))
# write problem in files :
with open(self.status_filename, "w") as f:
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
def setContactPlacementFromRBPRMState(phase, fb, stateId, limbs=None):
if limbs == None:
limbs = fb.limbs_names
for limbId in limbs:
eeName = fb.dict_limb_joint[limbId]
if fb.isLimbInContact(limbId, stateId):
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(
stateId, False, limbId)
placement = SE3.Identity()
placement.translation = np.matrix(p).T
if fb.cType == "_3_DOF":
normal = np.matrix(n).T
quat = Quaternion.FromTwoVectors(
np.matrix(fb.dict_normal[eeName]).T, normal)
else:
q_r = fb.getJointPosition(eeName)
quat = quatFromConfig(q_r)
placement.rotation = quat.matrix()
patch = contactPatchForEffector(phase, eeName)
patch.placement = placement
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
# init position at the origin, facing x axis
self.q_init[:3] = [0, 0, 1.]
self.q_init[-6] = self.v_init
# sample random position on a circle of radius random in [MIN_ROOT_DIST; MAX_ROOT_DIST]
random.seed()
radius = random.uniform(self.MIN_ROOT_DIST, self.MAX_ROOT_DIST)
alpha = random.uniform(0., 2. * np.pi)
print("Test on a circle, alpha = ", alpha)
print("Radius = ", radius)
self.q_goal = self.q_init[::]
self.q_goal[:3] = [radius * np.sin(alpha), -radius * np.cos(alpha), 1.]
if self.FINAL_ORIENTATION_RANDOM:
alpha = random.uniform(
0.,
2. * np.pi) # sample new random yaw value for the orientation
v_goal = np.matrix(
[radius * np.sin(alpha), -radius * np.cos(alpha), 0]).T
else:
v_goal = np.matrix([self.q_goal[0], self.q_goal[1],
0]).T # direction root_init -> root_goal
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
quat = Quaternion.FromTwoVectors(vx, v_goal)
self.q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
self.q_goal[-6] = self.v_goal * np.sin(alpha)
self.q_goal[-5] = -self.v_goal * np.cos(alpha)
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
print("q_init= " + str(self.q_init))
print("q_goal= " + str(self.q_goal))
# write problem in files :
with open(self.status_filename, "w") as f:
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
def contactPlacementFromRBPRMState(fb, stateId, eeName):
"""
Compute the placement of eeName for the given stateId
:param fb: an instance of rbprm.FullBody
:param stateId: the Id of the state
:param eeName: the name of the effector
:return: the placement represented as a pinocchio.SE3 object
"""
# get limbName from effector name :
fb.setCurrentConfig(fb.getConfigAtState(stateId))
placement = SE3FromConfig(fb.getJointPosition(eeName))
if fb.cType == "_3_DOF":
limbId = list(fb.dict_limb_joint.keys())[list(
fb.dict_limb_joint.values()).index(eeName)]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(
stateId, False, limbId)
normal = np.array(n)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]),
normal)
placement.rotation = quat.matrix()
return placement
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] += random.uniform(eff_x_range[0], eff_x_range[1])
p[1] += random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
# force root orientation : (align current z axis with vertical)
if success:
quat_1 = Quaternion(s1.q()[6], s1.q()[3], s1.q()[4], s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1, vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
success = s1.projectToRoot(q_root)
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
random.seed()
self.q_init[:3] = [
random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
]
self.q_goal = self.q_init[::]
for i in range(random.randint(0, 1000)):
random.uniform(0., 1.)
self.q_goal[:3] = [
random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
]
# compute the orientation such that q_init face q_goal :
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_init = np.matrix([
self.q_goal[0] - self.q_init[0], self.q_goal[1] - self.q_init[1], 0
]).T
quat = Quaternion.FromTwoVectors(vx, v_init)
self.q_init[3:7] = quat.coeffs().tolist()
self.q_goal[3:7] = self.q_init[3:7]
self.v(self.q_init)
print("initial root position : ", self.q_init[:3])
print("final root position : ", self.q_goal[:3])
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
# write problem in files :
with open(self.status_filename, "w") as f:
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
q_init[3:7] = [0, 0, 0, 1]
q_init[-6] = vInit
# sample random position on a circle of radius 2m
radius = 1.
import random
random.seed()
#alpha = random.uniform(0.,2.*np.pi)
alpha = random.uniform(0., 2. * np.pi)
print "Test on a circle, alpha = ", alpha
q_goal = q_init[::]
q_goal[0:3] = [radius * np.sin(alpha), -radius * np.cos(alpha), 1.]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([q_goal[0], q_goal[1], 0]).T
quat = Quaternion.FromTwoVectors(vx, v_goal)
q_goal[3:7] = quat.coeffs().T.tolist()[0]
# set final velocity to fix the orientation :
q_goal[-6] = vGoal * np.sin(alpha)
q_goal[-5] = -vGoal * np.cos(alpha)
v(q_goal)
print "initial root position : ", q_init
print "final root position : ", q_goal
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
# write problem in files :
f = open(statusFilename, "w")
f.write("q_init= " + str(q_init) + "\n")
f.write("q_goal= " + str(q_goal) + "\n")
f.close()
# y_goal must be random inside Y_BOUNDS but such that the line between q_init and q_goal is between +- MAX_ANGLE radian from the x axis
y_bound_goal = Y_BOUNDS[::]
y_angle_max = math.sin(MAX_ANGLE)*abs(x_init-x_goal) + y_init
y_angle_min = math.sin(-MAX_ANGLE)*abs(x_init-x_goal) + y_init
y_bound_goal[0] = max(y_angle_min,y_bound_goal[0])
y_bound_goal[1] = min(y_angle_max,y_bound_goal[1])
y_goal = random.uniform(y_bound_goal[0],y_bound_goal[1])
# compute the orientation such that q_init face q_goal :
# set final orientation to be along the circle :
vx = np.matrix([1,0,0]).T
v_init = np.matrix([x_goal-x_init,y_goal-y_init,0]).T
quat = Quaternion.FromTwoVectors(vx,v_init)
q_init[3:7] = quat.coeffs().T.tolist()[0]
q_goal=q_init[::]
q_goal[0:2] = [x_goal,y_goal]
print "initial root position : ",q_init
print "final root position : ",q_goal
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# write problem in files :
f = open(statusFilename,"w")
f.write("q_init= "+str(q_init)+"\n")
f.write("q_goal= "+str(q_goal)+"\n")
f.close()
def quatFromAlpha(alpha):
vx = np.matrix([1, 0, 0]).T
v = Vector3FromAlpha(alpha)
return Quaternion.FromTwoVectors(vx, v)
