class Transform (object):
def __init__ (self, *args):
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__ (self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform (other.translation) +\
self.translation
return Transform (rot, trans)
def inverse (self):
rot = self.quaternion.conjugate()
trans = - self.quaternion.conjugate().transform (self.translation)
return Transform (rot, trans)
def __str__ (self):
return "quaternion: %s,\ntranslation: %s"%(self.quaternion,
self.translation)
class Transform(object):
def __init__(self, *args):
if len(args) == 0:
self.translation = np.array([0.0, 0.0, 0.0])
self.quaternion = Quaternion()
if len(args) == 7:
self.translation = np.array(args[0:3])
self.quaternion = Quaternion(args[3:7])
if len(args) == 2:
self.quaternion = Quaternion(args[0])
self.translation = np.array(args[1])
if len(args) == 1:
if isinstance(args[0], Transform):
self.quaternion = args[0].quaternion
self.translation = args[0].translation
elif isinstance(args[0], (list, tuple)):
self.translation = np.array(args[0][0:3])
self.quaternion = Quaternion(args[0][3:7])
self.quaternion.normalize()
def __mul__(self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform(other.translation) + self.translation
return Transform(rot, trans)
def inverse(self):
rot = self.quaternion.conjugate()
trans = -self.quaternion.conjugate().transform(self.translation)
return Transform(rot, trans)
def transform(self, v):
res = self.quaternion.transform(v) + self.translation
return res
def __str__(self):
return "quaternion: %s,\ntranslation: %s" % (self.quaternion,
self.translation)
def __getitem__(self, i):
if i < 3:
return self.translation[i]
if i < 7:
return self.quaternion.toTuple()[i - 3]
raise IndexError("index out of range")
def __len__(self):
return 7
def toHomogeneousMatrix(self):
H = np.eye(4)
H[:3, :3] = self.quaternion.toRotationMatrix()
H[:3, 3] = self.translation
return H
def toTuple(self):
return tuple(self.translation) + self.quaternion.toTuple()
class Transform (object):
def __init__ (self, *args):
if len (args) == 0:
self.translation = np.array ([0.,0.,0.])
self.quaternion = Quaternion ()
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__ (self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform (other.translation) +\
self.translation
return Transform (rot, trans)
def inverse (self):
rot = self.quaternion.conjugate()
trans = - self.quaternion.conjugate().transform (self.translation)
return Transform (rot, trans)
def transform (self, v):
res = self.quaternion.transform (v) + self.translation
return res
def __str__ (self):
return "quaternion: %s,\ntranslation: %s"%(self.quaternion,
self.translation)
def __getitem__ (self, i):
if i<3: return self.translation [i]
if i<7: return self.quaternion.toTuple () [i-3]
raise IndexError ("index out of range")
def __len__ (self):
return 7
def toHomogeneousMatrix (self):
H = np.eye(4)
H[:3,:3] = self.quaternion.toRotationMatrix()
H[:3,3] = self.translation
return H
def toTuple (self):
return tuple (self.translation) + self.quaternion.toTuple ()
def flipBox(solver, pathId=None):
ps = solver.ps
if pathId is None: pathId = ps.numberPaths() - 1
q1 = ps.configAtParam(pathId, ps.pathLength(pathId))
q2 = q1[::]
rank = ps.robot.rankInConfiguration["box/root_joint"]
q2 [rank + 3 : rank + 7] = (
Quaternion([0, 1, 0, 0]) * Quaternion(q1[rank + 3 : rank + 7])).\
toTuple()
ps.resetGoalConfigs()
ps.setInitialConfig(q1)
ps.addGoalConfig(q2)
ps.setMaxIterPathPlanning(1000)
return solver.solve()
def __init__ (self, *args):
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__(self, other):
if not isinstance(other, Transform):
raise TypeError("expecting Transform type object")
trans = self.trans + (self.quat * Quaternion(0, other.trans) *
self.quat.conjugate()).array[1:]
quat = self.quat * other.quat
return Transform(quat, trans)
def computeRobotPositionFreeflyer(self):
jointMotion = Transform(Quaternion(self.robotConfig[3:7]),
self.robotConfig[0:3])
pos = self.rootJointPosition * jointMotion
self.transform.transform.rotation = (pos.quat.array[1], pos.quat.array[2],
pos.quat.array[3], pos.quat.array[0])
self.transform.transform.translation = (pos.trans[0], pos.trans[1],
pos.trans[2])
self.js.position = self.robotConfig[7:len(self.js.name) + 7]
def generateGoalFrom(self, qEstimated, qDesiredRobot):
qgoal = qEstimated[:]
rankO = self.ps.robot.rankInConfiguration["box/root_joint"]
rankT = self.ps.robot.rankInConfiguration["table/root_joint"]
# Rotate the box.
qT = Quaternion(qEstimated[rankO + 3:rankO + 7])
qgoal[rankO + 3:rankO + 7] = (qT * Quaternion([0, 0, 1, 0])).toTuple()
res, qgoal, err = self.graph.generateTargetConfig(
"starting_motion", qgoal, qgoal)
success = "free" == self.graph.getNode(qgoal)
if not res or not success:
print("Could not generate goal")
qgoalInStartingState = (qDesiredRobot[:min(rankO, rankT)] +
qgoal[min(rankO, rankT):])
# TODO if a transition from free to starting_state is added
# change the order of the arguments.
# res, pid, msg = ps.directPath (qgoal, qgoalInStartingState, True)
# self.tryDirectPaths (((qgoalInStartingState, qgoal),))
return qgoal, qgoalInStartingState
def computeRobotPositionPlanar(self):
theta = .5 * self.robotConfig[2]
jointMotion = Transform(
Quaternion(cos(theta), 0, 0, sin(theta)),
np.array([self.robotConfig[0], self.robotConfig[1], 0]))
pos = self.rootJointPosition * jointMotion
self.transform.transform.rotation = (pos.quat.array[1], pos.quat.array[2],
pos.quat.array[3], pos.quat.array[0])
self.transform.transform.translation = (pos.trans[0], pos.trans[1],
pos.trans[2])
self.js.position = self.robotConfig[3:len(self.js.name) + 3]
def get_visual_tag(self, tsmsg):
stamp = tsmsg.header.stamp
if stamp < self.current_stamp: return
rot = tsmsg.transform.rotation
# Compute scalar product between Z axis of camera and of tag.
# TODO Add a weight between translation and orientation
# It should depend on:
# - the distance (the farthest, the hardest it is to get the orientation)
distW = 1.
# - the above scalar product (the closest to 0, the hardest it is to get the orientation)
from hpp import Quaternion
from numpy import array
oriW = -Quaternion([
rot.x,
rot.y,
rot.z,
rot.w,
]).transform(array([0, 0, 1]))[2]
# - the tag size (for an orthogonal tag, an error theta in orientation should be considered
# equivalent to an position error of theta * tag_size)
tagsize = 0.063 * 4 # tag size * 4
s = tagsize * oriW * distW
try:
self.mutex.acquire()
j1 = tsmsg.header.frame_id
j2 = tsmsg.child_frame_id
if j1.endswith("_measured"): j1 = j1[:-len("_measured")]
if j2.endswith("_measured"): j2 = j2[:-len("_measured")]
if "/" not in j1: j1 = self.robot_name + "/" + j1
if "/" not in j2: j2 = self.robot_name + "/" + j2
names = self._get_transformation_constraint(j1,
j2,
tsmsg.transform,
prefix="",
orientationWeight=s)
# If this tag is in the next image:
if self.current_stamp < stamp:
# Assume no more visual tag will be received from image at time current_stamp.
self.last_stamp = self.current_stamp
self.last_visual_tag_constraints = self.current_visual_tag_constraints
# Reset for next image.
self.current_stamp = stamp
self.current_visual_tag_constraints = list()
self.last_stamp_is_ready = True
self.current_visual_tag_constraints.extend(names)
finally:
self.mutex.release()
def __init__(self, *args):
if len(args) == 0:
self.translation = np.array([0.0, 0.0, 0.0])
self.quaternion = Quaternion()
if len(args) == 7:
self.translation = np.array(args[0:3])
self.quaternion = Quaternion(args[3:7])
if len(args) == 2:
self.quaternion = Quaternion(args[0])
self.translation = np.array(args[1])
if len(args) == 1:
if isinstance(args[0], Transform):
self.quaternion = args[0].quaternion
self.translation = args[0].translation
elif isinstance(args[0], (list, tuple)):
self.translation = np.array(args[0][0:3])
self.quaternion = Quaternion(args[0][3:7])
self.quaternion.normalize()
def _get_transformation_constraint(self,
joint1,
joint2,
transform,
prefix="",
orientationWeight=1.):
hpp = self.hpp()
# Create a relative transformation constraint
j1 = joint1
j2 = joint2
name = prefix + j1 + "_" + j2
T = [
transform.translation.x,
transform.translation.y,
transform.translation.z,
transform.rotation.x,
transform.rotation.y,
transform.rotation.z,
transform.rotation.w,
]
if orientationWeight == 1.:
names = [
"T_" + name,
]
hpp.problem.createTransformationConstraint(names[0], j1, j2, T, [
True,
] * 6)
else:
from hpp import Quaternion
names = ["P_" + name, "sO_" + name]
hpp.problem.createPositionConstraint(names[0], j1, j2, T[:3],
[0, 0, 0], [
True,
] * 3)
hpp.problem.createOrientationConstraint(
"O_" + name, j1, j2,
Quaternion(T[3:]).inv().toTuple(), [
True,
] * 3)
hpp.problem.scCreateScalarMultiply(names[1], orientationWeight,
"O_" + name)
return names
def __init__(self, robot):
self.tf_root = robot.tf_root
try:
self.rootJointPosition = getRootJointPosition(robot)
except hpp.Error:
self.rootJointPosition = Transform(Quaternion([1, 0, 0, 0]),
np.array([0, 0, 0]))
self.referenceFrame = "map"
self.rootJointType = robot.rootJointType
if self.rootJointType == "freeflyer":
jointNames = robot.jointNames[4:]
self.computeRobotPosition = computeRobotPositionFreeflyer
elif self.rootJointType == "planar":
jointNames = robot.jointNames[3:]
self.computeRobotPosition = computeRobotPositionPlanar
elif self.rootJointType == "anchor":
jointNames = robot.jointNames
self.computeRobotPosition = computeRobotPositionAnchor
else:
raise RuntimeError("Unknow root joint type: " + self.rootJointType)
self.pubRobots = dict()
self.pubRobots['robot'] = rospy.Publisher('/joint_states', JointState)
self.pubRobots ['marker'] = \
rospy.Publisher ('/visualization_marker_array', MarkerArray)
rospy.init_node('hpp', log_level=rospy.FATAL)
self.broadcaster = TransformBroadcaster()
self.js = JointState()
self.js.name = jointNames
# Create constant transformation between the map frame and the robot
# base link frame.
self.transform = TransformStamped()
self.transform.header.frame_id = self.referenceFrame
self.transform.child_frame_id = self.tf_root
# Create constant transformation between the map frame and the obstacle
# frame.
# Here, the obstacle can move in the map frame (see __call__, with the
# move q_obs) but is without any joint.
self.trans_map_obstacle = TransformStamped()
self.trans_map_obstacle.header.frame_id = "map"
self.trans_map_obstacle.child_frame_id = "obstacle_base"
self.objects = dict()
self.markerArray = MarkerArray()
self.oid = 0
from hpp import Quaternion
from math import sqrt, pi, sin, cos
import numpy as np
ok = True
for q in (
[0.5, -0.5, -0.5, -0.5],
[-0.5, -0.5, 0.5, -0.5],
[0, 0, 1 / sqrt(2), 1 / sqrt(2)],
[0, 0, -1 / sqrt(2), 1 / sqrt(2)],
[0, -1 / sqrt(2), 0, 1 / sqrt(2)],
):
q1 = Quaternion(q)
r, p, y = q1.toRPY()
q2 = Quaternion()
q2.fromRPY(r, p, y)
print("q1", str(q1))
print("q2", str(q2))
print("rpy", r, p, y)
qdiff = q1 * q2.inv()
if qdiff.array[3] < 0:
qdiff.array *= -1
if not (np.abs(qdiff.array - np.array([0, 0, 0, 1])) < 1e-6).all():
print("ERROR")
ok = False
print("")
assert ok
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setMaxIterPathPlanning(50)
if isSimulation:
ps.setInitialConfig(q_init)
if args.context == defaultContext:
# Define problem
res, q_init, err = graph.generateTargetConfig("Loop | f", q_init, q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
q_goal = q_init[::]
rank = robot.rankInConfiguration["box/root_joint"]
q_goal[rank + 3 : rank + 7] = (
Quaternion([0, 1, 0, 0]) * Quaternion(q_init[rank + 3 : rank + 7])).\
toTuple()
# res, q_goal, err = graph.applyNodeConstraints ('free', q_goal)
res, q_proj, err = graph.generateTargetConfig("Loop | f", q_goal, q_goal)
if not res:
raise RuntimeError("Failed to project goal configuration")
assert q_init[-7:] == q_goal[-7:]
solver = Solver(
ps,
graph,
q_init,
q_goal,
e_l_l1,
e_l_r1,
e_l_l2,
# Set Optimization parameters
ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setMaxIterPathPlanning(50)
# q_init = robot.shootRandomConfig ()
# Define problem
res, q_init, err = graph.generateTargetConfig("Loop | f", q_init, q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
q_goal = q_init[::]
rank = robot.rankInConfiguration["box/root_joint"]
q_goal[rank + 3:rank + 7] = (Quaternion([0, 1, 0, 0]) *
Quaternion(q_init[rank + 3:rank + 7])).toTuple()
# res, q_goal, err = graph.applyNodeConstraints ('free', q_goal)
res, q_proj, err = graph.generateTargetConfig("Loop | f", q_goal, q_goal)
if not res:
raise RuntimeError("Failed to project goal configuration")
assert q_init[-7:] == q_goal[-7:]
solver = Solver(
ps,
graph,
q_init,
q_goal,
e_l1,
e_r1,
e_l2,
setGuassianShooter(qrand)
shoot_pose = 0
shoot_cfg = 0
while True:
chessboard_Z = random.uniform(dist_from_camera[0],
dist_from_camera[1])
chessboard_X = ((x - projection_matrix[0, 2]) /
projection_matrix[0, 0] * chessboard_Z)
chessboard_Y = ((y - projection_matrix[1, 2]) /
projection_matrix[1, 1] * chessboard_Z)
chessboard_position = np.matrix(
[chessboard_X, chessboard_Y, chessboard_Z])
q = Quaternion().fromRPY(
random.uniform(-pi / 12.0, pi / 12.0),
random.uniform(-pi / 12.0, pi / 12.0),
random.uniform(-pi / 12.0, pi / 12.0),
)
shoot_pose += 1
R = q.toRotationMatrix()
if (R * chessboard_normal)[2] >= 0.0:
continue
Rt = np.hstack(
(R, (chessboard_position - camera_position).transpose()))
if not all([
isInImage(projectPoint(Rt,
np.matrix(pt).transpose()))
for pt in chessboard_pts
]):
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
import csv
from hpp import Quaternion
# Test construction from SO3 matrix
for _ in xrange (1000):
q1 = Quaternion (np.random.sample (4)).normalize ()
mat = q1.toRotationMatrix ()
q2 = Quaternion (mat)
if abs (q1-q2) > 1e-10 and abs (q1+q2) > 1e-10:
raise RuntimeError ("Error in conversion quaternion matrix")
# test addition
with open ('./test/add-quaternions.csv', 'r') as f:
r = csv.reader (f, delimiter=',')
iline=0
for line in r:
iline+=1
data = map (float, line)
q1 = Quaternion (data [0:4])
q2 = Quaternion (data [4:8])
q3 = Quaternion (data [8:12])
def getRootJointPosition(robot):
pos = robot.getRootJointPosition()
return Transform(Quaternion(pos[3:7]), np.array(pos[0:3]))
# Set variance to 0.05 for table
rank = robot.rankInVelocity[table.name + "/root_joint"]
sigma[rank:rank + 6] = 6 * [0.05]
robot.setCurrentVelocity(sigma)
ps.setParameter("ConfigurationShooter/Gaussian/useRobotVelocity", True)
ps.client.basic.problem.selectConfigurationShooter("Gaussian")
# Set Optimization parameters
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 2.0)
q_tag11_down = q_init[:]
q_tag11_up = q_init[:]
rankO = robot.rankInConfiguration["box/root_joint"]
qT = Quaternion(q_tag11_up[rankO + 3:rankO + 7])
q_tag11_up[rankO + 3:rankO + 7] = (qT * Quaternion([0, 0, 1, 0])).toTuple()
sys.exit(0)
# q_init = robot.shootRandomConfig ()
# Define problem
res, q_init, err = graph.applyNodeConstraints("free", q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
q_goal = q_init[::]
q_goal[-4:] = [-0.5, -0.5, -0.5, 0.5]
solver = Solver(ps, graph, q_init, q_goal, e1, e2, e3, e4, e14, e23)
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
import csv
from hpp import Quaternion
# Test construction from SO3 matrix
for _ in range(1000):
q1 = Quaternion(np.random.sample(4)).normalize()
mat = q1.toRotationMatrix()
q2 = Quaternion(mat)
if abs(q1 - q2) > 1e-10 and abs(q1 + q2) > 1e-10:
raise RuntimeError("Error in conversion quaternion matrix")
# test addition
with open('./test/add-quaternions.csv', 'r') as f:
r = csv.reader(f, delimiter=',')
iline = 0
for line in r:
iline += 1
data = list(map(float, line))
q1 = Quaternion(data[0:4])
q2 = Quaternion(data[4:8])
q3 = Quaternion(data[8:12])
