src/conntact/assembly_tools.py
def update_average_wrench(self) -> None:
        """Create a very simple moving average of the incoming wrench readings and store it as self.average.wrench.
        """

        
        self._average_wrench_gripper = self.filters.average_wrench(self.current_wrench.wrench)

        #Get current angle from gripper to hole:
        transform_world_rotation:TransformStamped = self.tf_buffer.lookup_transform('tool0', 'target_hole_position', rospy.Time(0), rospy.Duration(1.25))
        #We want to rotate this only, not reinterpret F/T components.
        #We reinterpret based on the position of the TCP (but ignore the relative rotation). In addition, the wrench is internally measured at the load cell and has a built-in transformation to tool0 which is 5cm forward. We have to undo that transformation to get accurate transformation.                       
        offset =Point(self.tool_data[self.activeTCP]["transform"].transform.translation.x, self.tool_data[self.activeTCP]["transform"].transform.translation.y, self.tool_data[self.activeTCP]["transform"].transform.translation.z - .05)

            
        transform_world_rotation.transform.translation = offset

        #Execute reinterpret-to-tcp and rotate-to-world simultaneously:
        self._average_wrench_world = AssemblyTools.transform_wrench(transform_world_rotation, self._average_wrench_gripper) #This works

        #Output the wrench for debug visualization
        guy = self.create_wrench([0,0,0], [0,0,0])
        guy.wrench = self._average_wrench_world
        # guy.header.frame_id = "tool0"
        guy.header.frame_id = "target_hole_position"
        # guy.header.frame_id =  self.reference_frames['tcp'].child_frame_id
        self._adj_wrench_pub.publish(guy)
Create a very simple moving average of the incoming wrench readings and store it as self.average.wrench.
        
Create a very simple moving average of the incoming wrench readings and store it as self.average.wrench.
Create a very simple moving average of the incoming wrench readings and store it as self . average . wrench .
def update_average_wrench(self) -> None:
        self._average_wrench_gripper = self.filters.average_wrench(self.current_wrench.wrench)
        transform_world_rotation:TransformStamped = self.tf_buffer.lookup_transform('tool0', 'target_hole_position', rospy.Time(0), rospy.Duration(1.25))
        offset =Point(self.tool_data[self.activeTCP]["transform"].transform.translation.x, self.tool_data[self.activeTCP]["transform"].transform.translation.y, self.tool_data[self.activeTCP]["transform"].transform.translation.z - .05)
        transform_world_rotation.transform.translation = offset
        self._average_wrench_world = AssemblyTools.transform_wrench(transform_world_rotation, self._average_wrench_gripper) 
        guy = self.create_wrench([0,0,0], [0,0,0])
        guy.wrench = self._average_wrench_world
        guy.header.frame_id = "target_hole_position"
        self._adj_wrench_pub.publish(guy)
def update_average_wrench ( self ) -> None : self . _average_wrench_gripper = self . filters . average_wrench ( self . current_wrench . wrench ) transform_world_rotation : TransformStamped = self . tf_buffer . lookup_transform ( 'tool0' , 'target_hole_position' , rospy . Time ( 0 ) , rospy . Duration ( 1.25 ) ) offset = Point ( self . tool_data [ self . activeTCP ] [ "transform" ] . transform . translation . x , self . tool_data [ self . activeTCP ] [ "transform" ] . transform . translation . y , self . tool_data [ self . activeTCP ] [ "transform" ] . transform . translation . z - .05 ) transform_world_rotation . transform . translation = offset self . _average_wrench_world = AssemblyTools . transform_wrench ( transform_world_rotation , self . _average_wrench_gripper ) guy = self . create_wrench ( [ 0 , 0 , 0 ] , [ 0 , 0 , 0 ] ) guy . wrench = self . _average_wrench_world guy . header . frame_id = "target_hole_position" self . _adj_wrench_pub . publish ( guy )
Create a very simple moving average of the incoming wrench readings and store it as self.average.wrench.
Create a very simple moving average of the incoming wrench readings and store it as self . average . wrench .
"""Create a very simple moving average of the incoming wrench readings and store it as self.average.wrench.
        """ #Get current angle from gripper to hole: #We want to rotate this only, not reinterpret F/T components. #We reinterpret based on the position of the TCP (but ignore the relative rotation). In addition, the wrench is internally measured at the load cell and has a built-in transformation to tool0 which is 5cm forward. We have to undo that transformation to get accurate transformation.                        #Execute reinterpret-to-tcp and rotate-to-world simultaneously: #This works #Output the wrench for debug visualization # guy.header.frame_id = "tool0" # guy.header.frame_id =  self.reference_frames['tcp'].child_frame_id
self