def spherical_linear_interpolation_fromR(R0, Rf, h): # See https://en.wikipedia.org/wiki/Slerp q0 = np.array(so3.quaternion(R0)) qf = np.array(so3.quaternion(Rf)) v0 = np.array(MathUtils.normalize(q0)) vf = np.array(MathUtils.normalize(qf)) dot = np.dot(v0, vf) if dot < 0.0: vf = -vf dot = -dot # TODO: Is this causing non rotation matrixes? dot_threshold = 0.995 if dot > dot_threshold: # print(f"WARNING: dot={dot} > threshold={dot_threshold}\t linearly interprolating") # If results are close, calculate new result linearly result = v0 + h * (vf - v0) result_as_list = result.tolist() q = MathUtils.normalize(result_as_list) else: theta_0 = np.arccos(dot) theta = theta_0 * h sin_theta = np.sin(theta) sin_theta_0 = np.sin(theta_0) s0 = np.cos(theta) - dot * sin_theta / sin_theta_0 s1 = sin_theta / sin_theta_0 q = (s0 * v0) + (s1 * vf) return so3.from_quaternion(q)
def split_db(self): filenames = [] precision = int(np.ceil(np.log10(len(self.db.keys())))) n = 0 for k in sorted(self.db.keys()): fname = os.path.splitext( self.filename)[0] + '_' + str(n).zfill(precision) filenames.append(fname) f = open(fname + '.csv', 'w') for pose in self.db[k]: f.write(','.join([str(d) for d in k])) f.write(',') values = [] pose = se3.from_homogeneous(pose) # saving pose.t values += pose[1] # saving pose.q q = np.array(so3.quaternion(pose[0])) values += list(q[1:4]) values.append(q[0]) f.write(','.join([str(v) for v in values])) f.write('\n') f.close() n = n + 1 return filenames
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None): """Launches a very simple program that spawns a box with dimensions specified in boxes_db. """ world = WorldModel() world.loadElement("data/terrains/plane.env") robot = make_moving_base_robot(robotname, world) xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform", default=se3.identity(), world=world, doedit=False) for box_dims, poses in box_db.db.items(): if world.numRigidObjects() > 0: world.remove(world.rigidObject(0)) obj = make_box(world, box_dims[0], box_dims[1], box_dims[2]) poses_filtered = [] R,t = obj.getTransform() obj.setTransform(R, [0, 0, box_dims[2]/2.]) w_T_o = np.array(se3.homogeneous(obj.getTransform())) p_T_h = np.array(se3.homogeneous(xform)) p_T_h[2][3] += 0.02 for pose in poses: w_T_p = w_T_o.dot(pose) w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h)) if CollisionTestPose(world, robot, obj, w_T_h_des_se3): pose_pp = se3.from_homogeneous(pose) t_pp = pose_pp[1] q_pp = so3.quaternion(pose_pp[0]) q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]] print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims elif w_T_p[2][3] <= 0.: print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table" else: poses_filtered.append(pose) print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses" # embed() # create a hand emulator from the given robot name module = importlib.import_module('plugins.' + robotname) # emulator takes the robot index (0), start link index (6), and start driver index (6) program = FilteredMVBBTesterVisualizer(box_dims, poses_filtered, world, p_T_h, module, box_db, links_to_check) vis.setPlugin(None) vis.setPlugin(program) program.reshape(800, 600) vis.show() # this code manually updates the visualization while vis.shown(): time.sleep(0.1) return
def save_simulation(self, box_dims, pose, h_T_o, q, c_p, c_f): if len(c_p) != self.n_l * 3: raise RuntimeError( 'Incorrect size of c_p when calling save_simulation()') if len(c_f) != self.n_l * 6: raise RuntimeError( 'Incorrect size of c_f when calling save_simulation()') if len(q) != self.n_dofs: raise RuntimeError( 'Incorrect size of q when calling save_simulation()') if not self.has_simulation(box_dims, pose): f = open(self.filename_simulated, 'a') values = list(box_dims) if isinstance(pose, np.ndarray): pose = se3.from_homogeneous(pose) # saving pose.t values += pose[1] # saving pose.q q_wxyz = np.array(so3.quaternion(pose[0])) values += list(q_wxyz[1:4]) values.append(q_wxyz[0]) if isinstance(h_T_o, np.ndarray): h_T_o = se3.from_homogeneous(h_T_o) # saving h_T_o.t values += h_T_o[1] # saving h_T_o.q q_wxyz = np.array(so3.quaternion(h_T_o[0])) values += list(q_wxyz[1:4]) values.append(q_wxyz[0]) # saving q values += list(q) values += list(c_p) values += list(c_f) f.write(','.join([str(v) for v in values])) f.write('\n') f.close() self._load_mvbbs_simulated()
def publish_object(world_, msg): # A simple function for publishing object frame, pose and twist # Getting the transform from world to object and object velocity obj = world_.rigidObject(world_.numRigidObjects() - 1) (R_obj, t_obj) = obj.getTransform() (w_obj, v_obj) = obj.getVelocity() quat_obj = so3.quaternion(R_obj) # Setting the transform message and broadcasting object frame msg.header.stamp = rospy.Time.now() msg.child_frame_id = object_frame_name msg.transform.translation.x = t_obj[0] msg.transform.translation.y = t_obj[1] msg.transform.translation.z = t_obj[2] msg.transform.rotation.x = quat_obj[1] msg.transform.rotation.y = quat_obj[2] msg.transform.rotation.z = quat_obj[3] msg.transform.rotation.w = quat_obj[0] global_vars.obj_broadcaster.sendTransform(msg) # Publishing the object pose and twist obj_pose = Pose() obj_pose.position.x = msg.transform.translation.x obj_pose.position.y = msg.transform.translation.y obj_pose.position.z = msg.transform.translation.z obj_pose.orientation.x = msg.transform.rotation.x obj_pose.orientation.y = msg.transform.rotation.y obj_pose.orientation.z = msg.transform.rotation.z global_vars.obj_pose_pub.publish(obj_pose) obj_twist = Twist() # Publishing null twist now # obj_twist.angular.x = w_obj[0] # obj_twist.angular.y = w_obj[1] # obj_twist.angular.z = w_obj[2] # obj_twist.linear.x = v_obj[0] # obj_twist.linear.y = v_obj[1] # obj_twist.linear.z = v_obj[2] obj_twist.angular.x = 0.0 obj_twist.angular.y = 0.0 obj_twist.angular.z = 0.0 obj_twist.linear.x = 0.0 obj_twist.linear.y = 0.0 obj_twist.linear.z = 0.0 global_vars.obj_twist_pub.publish(obj_twist)
def publish_palm(robot, msg): # Simple function for publishing the palm frame # Getting the transform from world to floating frame floating_link = robot.link( 5 ) # This id comes from trial and error (should be kuka coupler bottom) (R, t) = floating_link.getTransform() quat = so3.quaternion(R) # Setting the transform message and broadcasting palm frame msg.header.stamp = rospy.Time.now() msg.child_frame_id = floating_frame_name msg.transform.translation.x = t[0] msg.transform.translation.y = t[1] msg.transform.translation.z = t[2] msg.transform.rotation.x = quat[1] msg.transform.rotation.y = quat[2] msg.transform.rotation.z = quat[3] msg.transform.rotation.w = quat[0] global_vars.palm_broadcaster.sendTransform(msg)
def transform_average(Ts): t = vectorops.div(vectorops.add(*[T[1] for T in Ts]), len(Ts)) qs = [so3.quaternion(T[0]) for T in Ts] q = vectorops.div(vectorops.add(*qs), len(Ts)) return (so3.from_quaternion(q), t)
def to_Quaternion(klampt_so3): """From Klamp't so3 element to ROS Quaternion""" q = so3.quaternion(klampt_so3) ros_q = Quaternion() ros_q.w, ros_q.x, ros_q.y, ros_q.z = q return ros_q
def broadcast_tf(broadcaster, klampt_obj, frameprefix="klampt", root="world", stamp=0.): """Broadcasts Klampt frames to the ROS tf module. Args: broadcaster (tf.TransformBroadcaster): the tf broadcaster klampt_obj: the object to publish. Can be WorldModel, Simulator, RobotModel, SimRobotController, anything with a getTransform or getCurrentTransform method, or se3 element frameprefix (str): the name of the base frame for this object root (str): the name of the TF world frame. Note: If klampt_obj is a Simulator or SimRobotController, then its corresponding model will be updated. """ from klampt import WorldModel, Simulator, RobotModel, SimRobotController if stamp == 'now': stamp = rospy.Time.now() elif isinstance(stamp, (int, float)): stamp = rospy.Time(stamp) world = None if isinstance(klampt_obj, WorldModel): world = klampt_obj elif isinstance(klampt_obj, Simulator): world = klampt_obj.model() klampt_obj.updateModel() if world is not None: prefix = frameprefix for i in range(world.numRigidObjects()): T = world.rigidObject(i).getTransform() q = so3.quaternion(T[0]) broadcaster.sendTransform( T[1], (q[1], q[2], q[3], q[0]), stamp, root, prefix + "/" + world.rigidObject(i).getName()) for i in range(world.numRobots()): robot = world.robot(i) rprefix = prefix + "/" + robot.getName() broadcast_tf(broadcaster, robot, rprefix, root) return robot = None if isinstance(klampt_obj, RobotModel): robot = klampt_obj elif isinstance(klampt_obj, SimRobotController): robot = klampt_obj.model() robot.setConfig(klampt_obj.getSensedConfig()) if robot is not None: rprefix = frameprefix for j in range(robot.numLinks()): p = robot.link(j).getParent() if p < 0: T = robot.link(j).getTransform() q = so3.quaternion(T[0]) broadcaster.sendTransform( T[1], (q[1], q[2], q[3], q[0]), stamp, root, rprefix + "/" + robot.link(j).getName()) else: Tparent = se3.mul(se3.inv(robot.link(p).getTransform()), robot.link(j).getTransform()) q = so3.quaternion(Tparent[0]) broadcaster.sendTransform( Tparent[1], (q[1], q[2], q[3], q[0]), stamp, rprefix + "/" + robot.link(p).getName(), rprefix + "/" + robot.link(j).getName()) return transform = None if isinstance(klampt_obj, (list, tuple)): #assume to be an SE3 element. transform = klampt_obj elif hasattr(klampt_obj, 'getTransform'): transform = klampt_obj.getTransform() elif hasattr(klampt_obj, 'getCurrentTransform'): transform = klampt_obj.getCurrentTransform() else: raise ValueError("Invalid type given to broadcast_tf: ", klampt_obj.__class__.__name__) q = so3.quaternion(transform[0]) broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp, root, frameprefix) return
def normalize_rotation(R): """Given a matrix close to a proper (orthogonal) rotation matrix, returns a true orthogonal matrix.""" q = so3.quaternion(R) #normalizes return so3.from_quaternion(q)