From 223f792861c192016ce1d1a6f50cff70a99e8a06 Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Tue, 20 Sep 2022 12:31:54 -0400 Subject: [PATCH] Accept values from user --- stretch_ros_tutorials/fk_poe.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/stretch_ros_tutorials/fk_poe.py b/stretch_ros_tutorials/fk_poe.py index 7c5d2bd..93fe1af 100644 --- a/stretch_ros_tutorials/fk_poe.py +++ b/stretch_ros_tutorials/fk_poe.py @@ -4,9 +4,6 @@ import numpy as np from rclpy.node import Node from math import sin as s, cos as c -# Users should input joint values from the command line (joint_lift, joint_arm, joint_wrist_yaw) -# Robot should determine the position of link_grasp_center at home position when all joints are zero? - class ForwardKinematics(): def __init__(self, joint_lift, joint_arm, joint_wrist_yaw): @@ -58,12 +55,19 @@ class ForwardKinematics(): return np.matmul(E, M) -if __name__ == '__main__': - fk = ForwardKinematics(0.5, 0.2, -1.34) +def main(): + print("Enter the joint values for joint_lift, joint_arm, joint_wrist_yaw: 0.6, 0.45, 3.145 for example") + x, y, z = input("Enter here: ").split(',', 3) + joint_lift, joint_arm, joint_wrist_yaw = float(x), float(y), float(z) + fk = ForwardKinematics(joint_lift, joint_arm, joint_wrist_yaw) result = fk.compute_fk() print(result) +if __name__ == '__main__': + main() + + # import numpy as np # from math import sin as s, cos as c