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])