diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py index c3bc4744..76957985 100644 --- a/src/adam/casadi/__init__.py +++ b/src/adam/casadi/__init__.py @@ -4,3 +4,5 @@ from .computations import KinDynComputations from .casadi_like import CasadiLike +from .computations_parametric import KinDynComputationsParametric + diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py index 515e38a3..c94bb4e8 100644 --- a/src/adam/casadi/computations_parametric.py +++ b/src/adam/casadi/computations_parametric.py @@ -34,7 +34,7 @@ def __init__( n_param_link = len(link_name_list) self.density = cs.SX.sym("density", n_param_link) self.length_multiplier = cs.SX.sym("length_multiplier",n_param_link) - factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=lenght_multiplier, density=density) + factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=self.length_multiplier, density=self.density) model = Model.build(factory=factory, joints_name_list=joints_name_list) self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = self.rbdalgos.NDoF diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py index 0dd4bf9e..2fb7d510 100644 --- a/src/adam/model/__init__.py +++ b/src/adam/model/__init__.py @@ -3,4 +3,7 @@ from .std_factories.std_joint import StdJoint from .std_factories.std_link import StdLink from .std_factories.std_model import URDFModelFactory +from .parametric_factories.parametric_joint import ParmetricJoint +from .parametric_factories.parametric_link import ParametricLink +from .parametric_factories.parametric_model import URDFParametricModelFactory from .tree import Node, Tree diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py index 4c086cbe..bdefd2f0 100644 --- a/src/adam/model/parametric_factories/parametric_joint.py +++ b/src/adam/model/parametric_factories/parametric_joint.py @@ -5,7 +5,7 @@ from adam.core.spatial_math import SpatialMath from adam.model import Joint -from adam.model.parametric_factories import link_parametric +from adam.model.parametric_factories.parametric_link import ParametricLink class ParmetricJoint(Joint): """Parametric Joint class""" @@ -14,34 +14,33 @@ def __init__( self, joint: urdf_parser_py.urdf.Joint, math: SpatialMath, - parent_link:link_parametric, + parent_link:ParametricLink, idx: Union[int, None] = None, ) -> None: self.math = math self.name = joint.name - self.parent = jparent_link + self.parent = parent_link self.child = joint.child self.type = joint.joint_type self.axis = joint.axis self.limit = joint.limit self.idx = idx - - joint_offset = self.parent_link.compute_joint_offset(joint, self.parent_link.offset) + self.joint = joint + joint_offset = self.parent.compute_joint_offset(joint, self.parent.link_offset) self.offset = joint_offset - self.origin = self.modify(self.parent_link.offset) + self.origin = self.modify(self.parent.link_offset) def modify(self, parent_joint_offset): - length = self.parent_link.get_principal_lenght_parametric() + length = self.parent.get_principal_lenght_parametric() # Ack for avoiding depending on casadi - vo = self.parent_link.origin[2] + vo = self.parent.origin[2] xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - xyz_rpy[0] = self.joint.origin[0,3] - xyz_rpy[1] = self.joint.origin[1,3] - xyz_rpy[2] = self.joint.origin[2,3] - xyz_rpy_temp= matrix_to_xyz_rpy(self.joint.origin) - xyz_rpy[3] = xyz_rpy_temp[3] - xyz_rpy[4] = xyz_rpy_temp[4] - xyz_rpy[5] = xyz_rpy_temp[5] + xyz_rpy[0] = self.joint.origin.xyz[0] + xyz_rpy[1] = self.joint.origin.xyz[1] + xyz_rpy[2] = self.joint.origin.xyz[2] + xyz_rpy[3] = self.joint.origin.rpy[0] + xyz_rpy[4] = self.joint.origin.rpy[1] + xyz_rpy[5] = self.joint.origin.rpy[2] if(xyz_rpy[2]<0): xyz_rpy[2] = -length +parent_joint_offset - self.offset diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py index ab4564fa..fcb70afb 100644 --- a/src/adam/model/parametric_factories/parametric_link.py +++ b/src/adam/model/parametric_factories/parametric_link.py @@ -5,6 +5,8 @@ from adam.core.spatial_math import SpatialMath from adam.model import Link +import math + class I_parametric(): def __init__(self) -> None: self.ixx = 0.0 @@ -25,6 +27,7 @@ class Side(Enum): WIDTH = 1 HEIGHT = 2 DEPTH = 3 +# ['XML_REFL', '_Link__get_collision', '_Link__get_visual', '_Link__set_collision', '_Link__set_visual', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'add_aggregate', 'add_aggregates_to_xml', 'aggregate_init', 'aggregate_order', 'aggregate_type', 'check_valid', 'collision', 'collisions', 'from_xml', 'from_xml_file', 'from_xml_string', 'get_aggregate_list', 'get_refl_vars', 'inertial', 'lump_aggregates', 'name', 'origin', 'parse', 'post_read_xml', 'pre_write_xml', 'read_xml', 'remove_aggregate', 'to_xml', 'to_xml_string', 'to_yaml', 'visual', 'visuals', 'write_xml'] class ParametricLink(Link): @@ -35,7 +38,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul self.name = link.name self.length_multiplier = length_multiplier self.density = density - self.visuals = link.visuals + self.visuals = link.visual + print(self.get_geometry(self.visuals)) self.geometry_type, self.visual_data = self.get_geometry(self.visuals) self.link = link self.link_offset = self.compute_offset() @@ -49,9 +53,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul link.inertial.origin.xyz = [0, 0, 0] link.inertial.origin.rpy = [0, 0, 0] - def get_principal_lenght(self): - visual = self.get_visual() - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + def get_principal_lenght(self): + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] if self.geometry_type == Geometry.CYLINDER: if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0): v_l = 2*self.visual_data.radius # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght @@ -66,8 +69,8 @@ def get_principal_lenght(self): return v_l def get_principal_lenght_parametric(self): - visual = self.get_visual() - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] if self.geometry_type == Geometry.CYLINDER: if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0): v_l = 2*self.visual_data_new[1] # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght @@ -82,8 +85,7 @@ def get_principal_lenght_parametric(self): return v_l def compute_offset(self): - visual = self.get_visual() - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] v_l= self.get_principal_lenght() v_o = xyz_rpy[2] if(v_o<0): @@ -94,10 +96,10 @@ def compute_offset(self): def compute_joint_offset(self,joint_i, parent_offset): # Taking the principal direction i.e. the length - visual = self.get_visual() - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] v_l= self.get_principal_lenght() - j_0 = matrix_to_xyz_rpy(joint_i.origin)[2] + print(joint_i.origin.xyz) + j_0 = joint_i.origin.xyz[2] v_o = xyz_rpy[2] if(j_0<0): joint_offset_temp = -(v_l + j_0-parent_offset) @@ -109,13 +111,19 @@ def compute_joint_offset(self,joint_i, parent_offset): @staticmethod def get_geometry(visual_obj): - """Returns the geometry type and the corresponding geometry object for a given visual""" - if visual_obj.geometry.box is not None: - return [Geometry.BOX, visual_obj.geometry.box] - if visual_obj.geometry.cylinder is not None: - return [Geometry.CYLINDER, visual_obj.geometry.cylinder] - if visual_obj.geometry.sphere is not None: - return [Geometry.SPHERE, visual_obj.geometry.sphere] + print("visual obj",dir(visual_obj)) + print(type(visual_obj.geometry)) + print(visual_obj.name) + """Returns the geometry type and thez corresponding geometry object for a given visual""" + if type(visual_obj.geometry) is urdf_parser_py.urdf.Box: + print("IS A BOX") + return (Geometry.BOX, visual_obj.geometry) + if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder: + print("IS A CYLINDER") + return (Geometry.CYLINDER, visual_obj.geometry) + if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere: + print("IS A SPHERE") + return (Geometry.SPHERE, visual_obj.geometry) """Function that starting from a multiplier and link visual characteristics computes the link volume""" def compute_volume(self): @@ -123,18 +131,18 @@ def compute_volume(self): """Modifies a link's volume by a given multiplier, in a manner that is logical with the link's geometry""" if self.geometry_type == Geometry.BOX: visual_data_new =[0.0, 0.0, 0.0] - visual_data_new[0] = self.visual_data.size[0] * self.length_multiplier[0] - visual_data_new[1] = self.visual_data.size[1] * self.length_multiplier[1] - visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier[2] + visual_data_new[0] = self.visual_data.size[0] #* self.length_multiplier[0] + visual_data_new[1] = self.visual_data.size[1] #* self.length_multiplier[1] + visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2] elif self.geometry_type == Geometry.CYLINDER: visual_data_new = [0.0, 0.0] - visual_data_new[0] = self.visual_data.length * self.length_multiplier[0] - visual_data_new[1] = self.visual_data.radius * self.length_multiplier[1] + visual_data_new[0] = self.visual_data.length * self.length_multiplier + visual_data_new[1] = self.visual_data.radius #* self.length_multiplier[1] volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] elif self.geometry_type == Geometry.SPHERE: visual_data_new = 0.0 - visual_data_new = self.visual_data.radius * self.length_multiplier[0] + visual_data_new = self.visual_data.radius * self.length_multiplier volume = 4 * math.pi * visual_data_new ** 3 / 3 return volume, visual_data_new @@ -147,20 +155,18 @@ def compute_mass(self): def modify_origin(self): origin = [0.0,0.0,0.0,0.0,0.0,0.0] - visual = self.get_visual() - """Modifies the position of the origin by a given amount""" - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] v_o = xyz_rpy[2] length = self.get_principal_lenght_parametric() if(v_o<0): - origin[2] = self.offset-length/2 + origin[2] = self.link_offset-length/2 origin[0] = xyz_rpy[0] origin[1] = xyz_rpy[1] origin[3] = xyz_rpy[3] origin[4] = xyz_rpy[4] origin[5] = xyz_rpy[5] else: - origin[2] = length/2 +self.offset + origin[2] = length/2 +self.link_offset origin[0] = xyz_rpy[0] origin[1] = xyz_rpy[1] origin[3] = xyz_rpy[3] @@ -173,8 +179,7 @@ def modify_origin(self): def compute_inertia_parametric(self): I = I_parametric - visual = self.get_visual() - xyz_rpy = matrix_to_xyz_rpy(visual.origin) + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] """Calculates inertia (ixx, iyy and izz) with the formula that corresponds to the geometry Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia""" if self.geometry_type == Geometry.BOX: diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py index 96556042..e5cf0342 100644 --- a/src/adam/model/parametric_factories/parametric_model.py +++ b/src/adam/model/parametric_factories/parametric_model.py @@ -4,7 +4,8 @@ import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath -from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint, LinkParametric, JointParametric +from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint +from adam.model import ParmetricJoint, ParametricLink class URDFParametricModelFactory(ModelFactory): @@ -20,7 +21,7 @@ def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, leng path = pathlib.Path(path) if not path.exists(): raise FileExistsError(path) - self.link_parametric_list = list + self.link_parametric_list = link_parametric_list self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path) self.name = self.urdf_desc.name self.lenght_multiplier = lenght_multiplier @@ -60,8 +61,8 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint: if(joint.parent in self.link_parametric_list): index_link = self.link_parametric_list.index(joint.parent) link_parent = self.get_element_by_name(joint.parent) - parent_link_parametric = LinkParametric(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) - return JointParametric(joint, self.math, parent_link_parametric) + parent_link_parametric = ParametricLink(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) + return ParmetricJoint(joint, self.math, parent_link_parametric) return StdJoint(joint, self.math) @@ -75,7 +76,7 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link: """ if(link.name in self.link_parametric_list): index_link = self.link_parametric_list.index(link.name) - return LinkParametric(link, self.math, self.lenght_multiplier[index_link], self.density[index_link]) + return ParametricLink(link, self.math, self.lenght_multiplier[index_link], self.density[index_link]) return StdLink(link, self.math) def get_element_by_name(self,link_name): diff --git a/tests/test_CasADi_computations_parametric.py b/tests/test_CasADi_computations_parametric.py new file mode 100644 index 00000000..6740d082 --- /dev/null +++ b/tests/test_CasADi_computations_parametric.py @@ -0,0 +1,194 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging +from os import link + +import casadi as cs +import numpy as np +# import pytest +import math +from adam.casadi.computations_parametric import KinDynComputationsParametric +from adam.casadi import KinDynComputations + +from adam.geometry import utils + +model_path ="/home/carlotta/iit_ws/element_hardware-intelligence/Software/OptimizationControlAndHardware/models/model.urdf" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] +def ComputeOriginalDensity(kinDyn, link_name): + link_original = kinDyn.rbdalgos.model.get_element_by_name(link_name, kinDyn.robot_desc) + mass = link_original.inertial.mass + volume = 0 + visual_obj = link_original.visuals[0] + if visual_obj.geometry.box is not None: + width = link_original.visuals[0].geometry.box.size[0] + depth = link_original.visuals[0].geometry.box.size[2] + height = link_original.visuals[0].geometry.box.size[1] + volume = width*depth*height + if visual_obj.geometry.cylinder is not None: + length = link_original.visuals[0].geometry.cylinder.length + radius = link_original.visuals[0].geometry.cylinder.radius + volume = math.pi*radius**2*length + if visual_obj.geometry.sphere is not None: + radius = link_original.visuals[0].geometry.sphere.radius + volume = 4*(math.pi*radius**3)/3 + return mass/volume + +def SX2DM(x): + return cs.DM(x) + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) + +link_name_list = ['r_upper_leg','r_lower_leg', 'r_hip_1', 'r_hip_2', 'r_ankle_1', 'r_ankle_2','l_upper_leg','l_lower_leg', 'l_hip_1', 'l_hip_2', 'l_ankle_1', 'l_ankle_2', 'l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1','r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1'] +comp_w_hardware = KinDynComputationsParametric(model_path, joints_name_list, link_name_list, root_link) +original_density = [] +for item in link_name_list: + original_density += [ComputeOriginalDensity(comp_w_hardware,item)] + +original_length = np.ones([len(link_name_list),3]) + +joint_type = np.zeros(len(joints_name_list)) + +n_dofs = len(joints_name_list) +# base pose quantities +xyz = (np.random.rand(3) - 0.5) * 5 +rpy = (np.random.rand(3) - 0.5) * 5 +base_vel = (np.random.rand(6) - 0.5) * 5 +# joints quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + +# # def test_mass_matrix(): +M = comp.mass_matrix_fun() +M_with_hardware = comp_w_hardware.mass_matrix_fun() +mass_test = SX2DM(M(H_b, s_)) +mass_test_hardware = SX2DM(M_with_hardware(H_b,s_, original_density, original_length)) +print("M=",cs.sumsqr(mass_test - mass_test_hardware)) + # assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +# def test_CMM(): +Jcm = comp.centroidal_momentum_matrix_fun() +Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun() +Jcm_test = SX2DM(Jcm(H_b, s_)) +Jcm_test_hardware = SX2DM(Jcm_with_hardware(H_b,s_, original_density, original_length)) +print("Jcmm=",cs.sumsqr(Jcm_test - Jcm_test_hardware)) +# assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +# def test_CoM_pos(): +com_f = comp.CoM_position_fun() +com_with_hardware_f = comp_w_hardware.CoM_position_fun() +CoM_cs = SX2DM(com_f(H_b, s_)) +CoM_hardware = SX2DM(com_with_hardware_f(H_b,s_, original_density, original_length)) +print("CoM=",cs.sumsqr(CoM_cs - CoM_hardware)) + # assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +# def test_total_mass(): +mass = comp.get_total_mass(); +mass_hardware_fun = comp_w_hardware.get_total_mass() +mass_hardware = SX2DM(mass_hardware_fun(original_density, original_length)) +print("mass=",cs.sumsqr(mass - mass_hardware)) + # assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + +# # def test_jacobian(): +J_tot = comp.jacobian_fun("l_sole") +J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole") +J_test = SX2DM(J_tot(H_b, s_)) +J_test_hardware = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length)) +print("J=",cs.sumsqr(J_test- J_test_hardware)) +# assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +# def test_jacobian_non_actuated(): +J_tot = comp.jacobian_fun("head") +J_test = SX2DM(J_tot(H_b, s_)) +J_tot_with_hardware = comp_w_hardware.jacobian_fun("head") +J_tot_with_hardware_test = SX2DM(J_tot_with_hardware(H_b, s_, original_density, original_length)) +print("J=",cs.sumsqr(J_test - J_tot_with_hardware_test)) + # assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5) + + +# def test_fk(): +T = comp.forward_kinematics_fun("l_sole") +H_test = SX2DM(T(H_b, s_)) +T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole") +H_with_hardware_test = SX2DM(T_with_hardware(H_b, s_, original_density, original_length)) +print("fk=",cs.sumsqr(H_test - H_with_hardware_test)) + # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +# # def test_fk_non_actuated(): +T = comp.forward_kinematics_fun("head") +H_test = SX2DM(T(H_b, s_)) +T_with_hardware = comp_w_hardware.forward_kinematics_fun("head") +H_with_hardware_test = SX2DM(T_with_hardware(H_b,s_,original_density, original_length)) +print("fk=",cs.sumsqr(H_test - H_with_hardware_test)) + # assert H_with_hardware_test[:3,:3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + # assert H_with_hardware_test[:3,3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +# def test_bias_force(): +h = comp.bias_force_fun() +h_test = SX2DM(h(H_b, s_, vb_, s_dot_)) + +h_with_hardware = comp_w_hardware.bias_force_fun() +h_with_hardware_test = SX2DM(h_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length)) +print("h=",cs.sumsqr(h_with_hardware_test-h_test)) +# assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) + + +# def test_coriolis_term(): +C = comp.coriolis_term_fun() +C_test = SX2DM(C(H_b, s_, vb_, s_dot_)) +C_with_hardware = comp_w_hardware.coriolis_term_fun() +C_with_hardware_test = SX2DM(C_with_hardware(H_b, s_, vb_, s_dot_, original_density, original_length)) +print("C=",cs.sumsqr(C_test - C_with_hardware_test)) + # assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) + + +# def test_gravity_term(): +G = comp.gravity_term_fun() +G_test = SX2DM(G(H_b, s_)) +G_with_hardware = comp_w_hardware.gravity_term_fun() +G_with_hardware_test = G_with_hardware(H_b,s_, original_density, original_length) +print("G=",cs.sumsqr(G_test- G_with_hardware_test)) + # assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)