Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add sdf utilities to get NMF_TEMPLATE and BOUNDS #12

Merged
merged 4 commits into from
Jan 20, 2025
Merged

Add sdf utilities to get NMF_TEMPLATE and BOUNDS #12

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7ec794e
add sdf utilities to get nmf teplate and test sdf
Chuanfang-Neptune Nov 6, 2024
fe797ca
fix the bug in from_sdf
gizemozd Jan 18, 2025
879b702
move the sdf example from entire pipeline to leg IK script
gizemozd Jan 18, 2025
aac198b
fix error
gizemozd Jan 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 11 additions & 1 deletion examples/example_entire_pipeline.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
from seqikpy.leg_inverse_kinematics import LegInvKinSeq
from seqikpy.head_inverse_kinematics import HeadInverseKinematics
from seqikpy.data import BOUNDS, INITIAL_ANGLES, NMF_TEMPLATE, PTS2ALIGN
from seqikpy.utils import save_file
from seqikpy.utils import save_file, from_sdf

logging.basicConfig(
format=" %(asctime)s - %(levelname)s- %(message)s",
Expand All @@ -42,6 +42,11 @@ def parse_args():
action="store_true",
help="Plot the joint angles.",
)
parser.add_argument(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice, thanks for adding this argument!

Can you possibly specify how to run this example script with --sdf option included in the docstring above and why it might be needed?

"--sdf",
default=None,
help="SDF path for the template model.",
)
return parser.parse_args()


Expand All @@ -54,6 +59,11 @@ def parse_args():

paths = Path(path_name).rglob("pose-3d")

if args.sdf:
sdf_name = args.sdf
sdf_name += "/" if not sdf_name.endswith("/") else ""
NMF_TEMPLATE, BOUNDS = from_sdf(sdf_name)

for data_path in paths:

logging.info("Running code in %s", data_path)
Expand Down
13 changes: 13 additions & 0 deletions seqikpy/utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
import cv2
from scipy.interpolate import pchip_interpolate

import xml
import xml.etree.ElementTree as ET
import numpy as np

def get_fps_from_video(video_dir):
""" Finds the fps of a video. """
Expand Down Expand Up @@ -357,3 +360,13 @@ def interpolate_joint_angles(joint_angles_dict, **kwargs):
interpolated_joint_angles[dof] = interpolate_signal(signal=joint_angles_dict[dof], **kwargs)

return interpolated_joint_angles

def from_sdf(sdf_file):
sdf_in = ET.parse(sdf_file)
root_in = sdf_in.getroot()
model_in = root_in.find('world').find('model')
links = model_in.findall('link')
joints = model_in.findall('joint')
NMF_TEMPLATE = {link.attrib['name']:link.find('pose').text for link in links}
BOUNDS = {joint.attrib['name']:(joint.find('axis').find('limit').find('lower').text,joint.find('axis').find('limit').find('upper').text) for joint in joints}
return NMF_TEMPLATE, BOUNDS
Loading