From 996264e125b72747a84bf3f14a4a5e00bdaf26ff Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 12 Mar 2021 14:55:42 +0100 Subject: [PATCH] KDL based robot model --- controllers.py | 9 +++++---- model.py | 38 +++++++++++++++++++++++++++++++++++ simulation.py => robot_sim.py | 12 ----------- run_demo.py | 10 +++++---- utils.py | 30 +++++++++++++++++++++++++++ 5 files changed, 79 insertions(+), 20 deletions(-) create mode 100644 model.py rename simulation.py => robot_sim.py (86%) diff --git a/controllers.py b/controllers.py index 4d705df..7188191 100644 --- a/controllers.py +++ b/controllers.py @@ -4,16 +4,17 @@ import numpy as np class CartesianPoseController: def __init__(self, model): self.model = model + # self.x_d = x0 def set_target(self, pose): - self.target = pose.translation + self.x_d = pose.translation def update(self, q, dq): - t = self.model.pose().translation - J = self.model.jacobian(q) + t = self.model.pose(q).translation + J = self.model.jacobian(q)[:3, :] J_pinv = np.linalg.pinv(J) - err = 2.0 * (self.target - t) + err = 2.0 * (self.x_d - t) cmd = np.dot(J_pinv, err) return cmd diff --git a/model.py b/model.py new file mode 100644 index 0000000..283f560 --- /dev/null +++ b/model.py @@ -0,0 +1,38 @@ +import numpy as np +import kdl_parser_py.urdf as kdl_parser +import PyKDL as kdl + +import utils + + +class Model(object): + def __init__(self, root_frame_id, tip_frame_id): + _, tree = kdl_parser.treeFromFile("assets/urdfs/panda/panda_arm_hand.urdf") + chain = tree.getChain(root_frame_id, tip_frame_id) + self.nr_joints = chain.getNrOfJoints() + self.fk_pos_solver = kdl.ChainFkSolverPos_recursive(chain) + self.fk_vel_solver = kdl.ChainFkSolverVel_recursive(chain) + self.jac_solver = kdl.ChainJntToJacSolver(chain) + return + + def pose(self, q): + jnt_array = utils.to_kdl_jnt_array(q) + frame = kdl.Frame() + self.fk_pos_solver.JntToCart(jnt_array, frame) + return utils.Transform.from_kdl(frame) + + def velocities(self, q, dq): + jnt_array_vel = kdl.JntArrayVel( + utils.to_kdl_jnt_array(q), utils.to_kdl_jnt_array(dq) + ) + twist = kdl.FrameVel() + self.fk_vel_solver.JntToCart(jnt_array_vel, twist) + d = twist.deriv() + linear, angular = np.r_[d[0], d[1], d[2]], np.r_[d[3], d[4], d[5]] + return linear, angular + + def jacobian(self, q): + jnt_array = utils.to_kdl_jnt_array(q) + J = kdl.Jacobian(self.nr_joints) + self.jac_solver.JntToJac(jnt_array, J) + return utils.kdl_to_mat(J) diff --git a/simulation.py b/robot_sim.py similarity index 86% rename from simulation.py rename to robot_sim.py index 2ffaf9f..ac23ab2 100644 --- a/simulation.py +++ b/robot_sim.py @@ -48,18 +48,6 @@ class PandaArm(object): self.uid, i, p.VELOCITY_CONTROL, targetVelocity=velocity ) - def pose(self): - result = p.getLinkState(self.uid, self.ee_link_id, computeForwardKinematics=1) - _, _, _, _, frame_pos, frame_quat = result - T_world_ee = Transform(Rotation.from_quat(frame_quat), np.array(frame_pos)) - return self.T_world_base.inverse() * T_world_ee - - def jacobian(self, q): - q = np.r_[q, 0.0, 0.0].tolist() - q_dot, q_ddot = np.zeros(9).tolist(), np.zeros(9).tolist() - linear, _ = p.calculateJacobian(self.uid, 7, [0.0, 0.0, 0.0], q, q_dot, q_ddot) - return np.asarray(linear)[:, :7] - class PandaGripper(object): def move(self, width): diff --git a/run_demo.py b/run_demo.py index 4dcb90a..6ab38f4 100644 --- a/run_demo.py +++ b/run_demo.py @@ -1,7 +1,8 @@ import rospy from controllers import * -from simulation import * +from model import * +from robot_sim import * from utils import * @@ -12,11 +13,11 @@ dt = 1.0 / 60.0 rospy.init_node("demo") env = SimPandaEnv(gui) -model = env.arm +model = Model("panda_link0", "panda_link8") controller = CartesianPoseController(model) -init_ee_pose = env.arm.pose() -marker = InteractiveMarkerWrapper("target", "panda_link0", init_ee_pose) +q, dq = env.arm.get_state() +marker = InteractiveMarkerWrapper("target", "panda_link0", model.pose(q)) # run the control loop while True: @@ -24,4 +25,5 @@ while True: q, dq = env.arm.get_state() cmd = controller.update(q, dq) env.arm.set_desired_joint_velocities(cmd) + env.forward(dt) diff --git a/utils.py b/utils.py index d599456..1418dd7 100644 --- a/utils.py +++ b/utils.py @@ -1,5 +1,6 @@ import numpy as np from scipy.spatial.transform import Rotation +import PyKDL as kdl from interactive_markers.interactive_marker_server import * from interactive_markers.menu_handler import * @@ -109,3 +110,32 @@ class Transform(object): rotation = Rotation.from_quat(list[:4]) translation = list[4:] return cls(rotation, translation) + + @classmethod + def from_kdl(cls, f): + rotation = Rotation.from_matrix( + np.array( + [ + [f.M[0, 0], f.M[0, 1], f.M[0, 2]], + [f.M[1, 0], f.M[1, 1], f.M[1, 2]], + [f.M[2, 0], f.M[2, 1], f.M[2, 2]], + ] + ) + ) + translation = np.r_[f.p[0], f.p[1], f.p[2]] + return cls(rotation, translation) + + +def to_kdl_jnt_array(q): + jnt_array = kdl.JntArray(len(q)) + for i, q_i in enumerate(q): + jnt_array[i] = q_i + return jnt_array + + +def kdl_to_mat(m): + mat = np.zeros((m.rows(), m.columns())) + for i in range(m.rows()): + for j in range(m.columns()): + mat[i, j] = m[i, j] + return mat