type=str,
required=True,
help="ModuleFamily/ModuleName")
return parser.parse_args(args)
if __name__ == '__main__':
parser = parse_args(rospy.myargv(sys.argv[1:]))
rospy.init_node(name="effort_ctrl", anonymous=True)
# Set up HEBI ROS interface
hebi_families = parser.module.split('/')[0]
hebi_names = parser.module.split('/')[1]
hebi_wrap = HebirosWrapper(parser.hebi_group, [hebi_families],
[hebi_names])
hebi_mapping = [parser.module]
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.effort_gains.name = hebi_mapping
cmd_msg.settings.effort_gains.kp = [1]
cmd_msg.settings.effort_gains.ki = [0]
cmd_msg.settings.effort_gains.kd = [0]
cmd_msg.settings.effort_gains.i_clamp = [0]
hebi_wrap.send_command_with_acknowledgement(cmd_msg)
_hold_position = False
_hold_joint_effort = None
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
class HebiArmController(object):
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def execute(self, rate, joint_states, durations):
max_height = 0.0
while not rospy.is_shutdown():
if len(joint_states) > 0:
# start waypoint
self.start_wp.positions = self.position_fbk
self.start_wp.velocities = [0.0]*self.hebi_wrap.hebi_count
self.start_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count
# end waypoint
self.end_wp.positions = joint_states.pop(0)
self.end_wp.velocities = [0.0]*self.hebi_wrap.hebi_count
self.end_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count
self._hold_positions = self.end_wp.positions
# action goal
self.goal.times = [0.0, durations.pop()]
self._hold = False
self.hebi_wrap.trajectory_action_client.send_goal(self.goal)
self.hebi_wrap.trajectory_action_client.wait_for_result()
self._hold = True
# check status
if abs(self.position_fbk[0] - self.end_wp.positions[0]) > 0.0872665:
print("Failed to achieve objective position.")
break
else:
rospy.sleep(2.0)
fk = self.kdl_fk.forward(self.position_fbk)
max_height = fk.tolist()[2][3]
print("Max Height: {}".format(max_height))
break
rate.sleep()
if self.openmeta_testbench_manifest_path is not None:
steps_completed = 0
if 7 <= len(joint_states):
steps_completed = 0
elif 5 <= len(joint_states) < 7:
steps_completed = 1
elif 3 <= len(joint_states) < 5:
steps_completed = 2
elif len(joint_states) < 3:
steps_completed = 3
self._metrics['steps_completed'] = steps_completed
self._metrics['max_height'] = max_height
self._write_metrics_to_tb_manifest()
def _joint_state_cb(self, msg):
if not rospy.is_shutdown():
if self._hold:
self.jointstate.position = self._hold_positions
self.jointstate.velocity = []
self.hebi_wrap.joint_state_publisher.publish(self.jointstate)
self.position_fbk = self.hebi_wrap.get_joint_positions()
self.velocity_fbk = self.hebi_wrap.get_joint_velocities()
self.effort_fbk = self.hebi_wrap.get_joint_efforts()
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self.position_fbk
jointstate.velocity = self.velocity_fbk
jointstate.effort = self.effort_fbk
self._joint_state_pub.publish(jointstate)
def _write_metrics_to_tb_manifest(self):
# Write to testbench_manifest metric
for tb_metric in self._testbench_manifest['Metrics']:
if tb_metric['Name'] in self._metrics:
tb_metric['Value'] = self._metrics[tb_metric['Name']]
# Save updated testbench_manifest.json
with open(self.openmeta_testbench_manifest_path, 'w') as savefile:
json_str = json.dumps(self._testbench_manifest, sort_keys=True, indent=2, separators=(',', ': '))
savefile.write(json_str)
def main():
# ROS stuff
rospy.init_node('hexapod_leg_fsm_node', anonymous=True)
# Parse node args
parser = parse_args(sys.argv[1:])
hebi_group_name = parser.hebi_group_name
hebi_mapping_hip = parser.hebi_mapping_hip
hebi_mapping_knee = parser.hebi_mapping_knee
hebi_mapping_ankle = parser.hebi_mapping_ankle
base_link_name = parser.base_link_name
end_link_name = parser.end_link_name
from_master_topic = parser.from_master_topic
to_master_topic = parser.to_master_topic
step_height = parser.step_height
# Hardware interface
hebi_mapping = [hebi_mapping_hip, hebi_mapping_knee, hebi_mapping_ankle]
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = [5, 8, 2]
cmd_msg.settings.position_gains.ki = [0.001, 0.001, 0.001]
cmd_msg.settings.position_gains.kd = [0, 0, 0]
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
hebi_wrap.send_command_with_acknowledgement(cmd_msg)
# Get URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('robot_description'):
urdf_str = rospy.get_param('robot_description')
urdf_loaded = True
rospy.loginfo("Pulled {} from parameter server.".format(rospy.resolve_name('robot_description')))
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
### CREATE LEG STATE INSTANCES ###
wait_for_master = WaitForMaster(hebiros_wrapper=hebi_wrap, from_master_topic=from_master_topic, to_master_topic=to_master_topic)
step = Step(hebiros_wrapper=hebi_wrap, urdf_str=urdf_str, base_link=base_link_name, end_link=end_link_name, step_height=step_height)
push = Push(hebiros_wrapper=hebi_wrap, urdf_str=urdf_str, base_link=base_link_name, end_link=end_link_name)
### CREATE TOP SM ###
top = StateMachine(outcomes=['exit','success'])
### INITIALIZE USERDATA ###
top.userdata.prev_joint_pos = [0,0,0]
top.userdata.target_end_link_point = None
top.userdata.execution_time = None
top.userdata.active_joints = None
remapping_list = ['prev_joint_pos', 'prev_joint_pos', 'target_end_link_point', 'execution_time', 'active_joints']
remapping_dict = {userdata: userdata for userdata in remapping_list}
with top:
StateMachine.add('WAIT_FOR_MASTER', wait_for_master,
transitions={'exit':'exit', 'step':'STEP', 'push':'PUSH'},
remapping=remapping_dict)
StateMachine.add('STEP', step,
transitions={'ik_failed':'WAIT_FOR_MASTER', 'success':'WAIT_FOR_MASTER'},
remapping=remapping_dict)
StateMachine.add('PUSH', push,
transitions={'ik_failed':'WAIT_FOR_MASTER', 'success':'WAIT_FOR_MASTER'},
remapping=remapping_dict)
sis = smach_ros.IntrospectionServer(str(rospy.get_name()), top, '/SM_ROOT' + str(rospy.get_name()))
sis.start()
#user_input = raw_input("Please press the 'Return/Enter' key to start executing \
# - pkg: hexapod_leg_fsm_node.py | node: " + str(rospy.get_name()) + "\n")
print("Input received. Executing...")
top.execute()
rospy.spin()
sis.stop()
print("\nExiting " + str(rospy.get_name()))
def __init__(self, hebi_group_name, modules, base_link_name,
end_link_name):
self._hold_position = False
self._hold_joint_angles = []
# Set up HEBI ROS interface
self.hebi_families = [module.split('/')[0] for module in modules]
self.hebi_names = [module.split('/')[1] for module in modules]
self.hebi_wrap = HebirosWrapper(hebi_group_name, self.hebi_families,
self.hebi_names)
self.hebi_mapping = modules
# Set HEBI Gains # FIXME: Hardcoded for now # FIXME: Uncomment if simulating
"""
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping
cmd_msg.settings.name = self.hebi_mapping
cmd_msg.settings.position_gains.name = self.hebi_mapping
cmd_msg.settings.position_gains.kp = [1.3, 2.6, 2.5, 1, 0.8]
cmd_msg.settings.position_gains.ki = [0] * 5
cmd_msg.settings.position_gains.kd = [0] * 5
cmd_msg.settings.position_gains.i_clamp = [0] * 5
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
"""
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk = KDLKinematics(robot_urdf, base_link_name, end_link_name)
# trac-ik setup
self.trac_ik = IK(
base_link_name,
end_link_name,
urdf_string=urdf_str,
timeout=0.1,
epsilon=1e-4,
solve_type="Distance") # FIXME: Decrease timeout as able
# joint state publisher
# Wait for connections to be setup
while not rospy.is_shutdown() and len(
self.hebi_wrap.get_joint_positions()) < len(self.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._active_joints = self.kdl_fk.get_joint_names()
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
self.hebi_wrap.add_feedback_callback(self._hold_position_cb)
class ArmController(object):
def __init__(self, hebi_group_name, modules, base_link_name,
end_link_name):
self._hold_position = False
self._hold_joint_angles = []
# Set up HEBI ROS interface
self.hebi_families = [module.split('/')[0] for module in modules]
self.hebi_names = [module.split('/')[1] for module in modules]
self.hebi_wrap = HebirosWrapper(hebi_group_name, self.hebi_families,
self.hebi_names)
self.hebi_mapping = modules
# Set HEBI Gains # FIXME: Hardcoded for now # FIXME: Uncomment if simulating
"""
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping
cmd_msg.settings.name = self.hebi_mapping
cmd_msg.settings.position_gains.name = self.hebi_mapping
cmd_msg.settings.position_gains.kp = [1.3, 2.6, 2.5, 1, 0.8]
cmd_msg.settings.position_gains.ki = [0] * 5
cmd_msg.settings.position_gains.kd = [0] * 5
cmd_msg.settings.position_gains.i_clamp = [0] * 5
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
"""
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk = KDLKinematics(robot_urdf, base_link_name, end_link_name)
# trac-ik setup
self.trac_ik = IK(
base_link_name,
end_link_name,
urdf_string=urdf_str,
timeout=0.1,
epsilon=1e-4,
solve_type="Distance") # FIXME: Decrease timeout as able
# joint state publisher
# Wait for connections to be setup
while not rospy.is_shutdown() and len(
self.hebi_wrap.get_joint_positions()) < len(self.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._active_joints = self.kdl_fk.get_joint_names()
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
self.hebi_wrap.add_feedback_callback(self._hold_position_cb)
def _joint_state_cb(self, msg):
if not rospy.is_shutdown() and self._active_joints is not None:
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self.hebi_wrap.get_joint_positions()
jointstate.velocity = []
jointstate.effort = []
self._joint_state_pub.publish(jointstate)
def _hold_position_cb(self, msg):
if not rospy.is_shutdown() and self._hold_position:
jointstate = JointState()
jointstate.name = self.hebi_wrap.hebi_mapping
jointstate.position = self._hold_joint_angles
jointstate.velocity = []
jointstate.effort = []
self.hebi_wrap.joint_state_publisher.publish(jointstate)
def move_to_pose(self, target_pose, mode, move_duration=0):
"""
:param target_pose: [x,y,z,R,P,Y] or Pose msg
:type target_pose: list or Pose
:param mode: "trajectory" or "command"
:type mode: str
:param move_duration: time in seconds to complete move
:type move_duration: float
:return:
"""
xyz_bounds = [0.01, 0.01, 0.01]
wxyz_bounds = [0.05, 0.05, 0.05]
if not isinstance(target_pose, Pose):
# xyz
target_xyz = [0, 0, 0]
for i in range(3):
if target_pose[i] in ("", " "):
xyz_bounds[i] = 1e5
else:
target_xyz[i] = float(target_pose[i])
# RPY
target_rpy = [0, 0, 0] # Tait-Byran Extrinsic xyz Euler angles
for i in range(3):
if target_pose[3 + i] in ("", " "):
wxyz_bounds[i] = 1e5
else:
target_rpy[i] = float(target_pose[3 + i])
# wxyz
target_wxyz = transforms.quaternion_from_euler(
target_rpy[0], target_rpy[1], target_rpy[2],
axes='sxyz').tolist()
else:
target_xyz = [
target_pose.position.x, target_pose.position.y,
target_pose.position.z
]
target_wxyz = [
target_pose.orientation.w, target_pose.orientation.x,
target_pose.orientation.y, target_pose.orientation.z
]
print("Seed angles: {}".format(
[round(ang, 4) for ang in self.hebi_wrap.get_joint_positions()]))
print("Target End-Effector Pose...")
print(" xyz: {}".format(target_xyz))
print(" wxyz: {}".format(target_wxyz))
print("Bounds...")
print(" xyz: {}".format(xyz_bounds))
print(" wxyz: {}".format(wxyz_bounds))
# Get target joint angles
target_jt_angles = self.trac_ik.get_ik(
self.hebi_wrap.get_joint_positions(), target_xyz[0], target_xyz[1],
target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2],
target_wxyz[3], xyz_bounds[0], xyz_bounds[1], xyz_bounds[2],
wxyz_bounds[0], wxyz_bounds[1], wxyz_bounds[2])
if target_jt_angles is None:
print("NO JOINT SOLUTION FOUND FOR END-EFFECTOR POSE "
"\n(x: {}, y: {}, z: {}, W: {}, X: {}, Y: {}, Z: {})"
"\nPLEASE TRY AGAIN.".format(*target_xyz + target_wxyz))
return False
else:
if mode == "trajectory":
goal = TrajectoryGoal()
start_wp = WaypointMsg()
start_wp.names = self.hebi_mapping
start_wp.positions = self.hebi_wrap.get_joint_positions()
start_wp.velocities = [0] * len(self.hebi_mapping)
start_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(start_wp)
end_wp = WaypointMsg()
end_wp.names = self.hebi_mapping
end_wp.positions = target_jt_angles
end_wp.velocities = [0] * len(self.hebi_mapping)
end_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(end_wp)
goal.times.extend([0, move_duration])
self._hold_position = False
self.hebi_wrap.trajectory_action_client.send_goal_and_wait(
goal)
self._hold_joint_angles = self.hebi_wrap.get_joint_positions()
self._hold_position = True
else: # mode == "command"
cmd = JointState()
cmd.name = self.hebi_mapping
cmd.position = target_jt_angles
cmd.velocity = [0] * len(self.hebi_mapping)
cmd.effort = [0] * len(self.hebi_mapping)
self._hold_position = False
self.hebi_wrap.joint_state_publisher.publish(cmd)
self._hold_joint_angles = target_jt_angles
self._hold_position = True
return True
def get_end_effector_pose(self):
h_transform_mat = self.kdl_fk.forward(
self.hebi_wrap.get_joint_positions())
return _get_pose_from_homogeneous_matrix(h_transform_mat)