def __init__(self, env, use_mocap_ctrl=True, **kwargs):
super(YumiReachAgent, self).__init__(env, **kwargs)
import PyKDL
from gym.utils.kdl_parser import kdl_tree_from_urdf_model
assert isinstance(env.unwrapped, YumiEnv)
assert env.unwrapped.block_gripper
self._env = env
self._raw_env = env.unwrapped # type: YumiEnv
self._sim = self._raw_env.sim
self._dt = env.unwrapped.dt
self._goal = None
self._robot = YumiEnv.get_urdf_model()
self._kdl = PyKDL
self.use_mocap_ctrl = use_mocap_ctrl
# make sure the simulator is ready before getting the transforms
self._sim.forward()
self._base_offset = self._sim.data.get_body_xpos(
'yumi_base_link').copy()
kdl_tree = kdl_tree_from_urdf_model(self._robot)
base_link = self._robot.get_root()
ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base')
ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base')
if self._raw_env.has_right_arm:
self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center')
if self._raw_env.has_left_arm:
self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center')
self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r)
self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l)
self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r)
self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l)
def __init__(self,
env,
use_qp_solver=False,
check_joint_limits=False,
use_mocap_ctrl=True,
**kwargs):
super(YumiBarAgent, self).__init__(env, **kwargs)
import PyKDL
from gym.utils.kdl_parser import kdl_tree_from_urdf_model
from gym.envs.yumi.yumi_env import YumiEnv
assert isinstance(env.unwrapped, YumiEnv)
assert env.unwrapped.has_two_arms
assert not env.unwrapped.block_gripper
self._raw_env = env.unwrapped # type: YumiEnv
self._sim = self._raw_env.sim
self._dt = env.unwrapped.dt
self._goal = None
self._phase = 0
self._phase_steps = 0
self._locked_l_to_r_tf = None
self._robot = YumiEnv.get_urdf_model()
self._kdl = PyKDL
self.use_mocap_ctrl = use_mocap_ctrl
self.use_qp_solver = use_qp_solver
self.check_joint_limits = check_joint_limits
if check_joint_limits and not use_qp_solver:
raise ValueError(
'Joint limits can only be checked with the QP solver!')
# make sure the simulator is ready before getting the transforms
self._sim.forward()
self._base_offset = self._sim.data.get_body_xpos(
'yumi_base_link').copy()
kdl_tree = kdl_tree_from_urdf_model(self._robot)
base_link = self._robot.get_root()
ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base')
ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base')
if self._raw_env.has_right_arm:
self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center')
if self._raw_env.has_left_arm:
self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center')
self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r)
self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l)
self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r)
self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l)
self._jac_solver_r = PyKDL.ChainJntToJacSolver(ee_arm_chain_r)
self._jac_solver_l = PyKDL.ChainJntToJacSolver(ee_arm_chain_l)
def __init__(self):
self.dh_params = [[-0.033, pi / 2, 0.145, pi, -1],
[0.155, 0, 0, pi / 2, -1],
[0.135, 0, 0, 0.0, -1],
[-0.002, pi / 2, 0, -pi / 2, -1],
[0, pi, -0.185, -pi, -1]]
self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180]
self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180]
self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180]
# Setup the subscribers for the joint states
self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback,
queue_size=5)
# TF2 broadcaster
self.pose_broadcaster = tf2_ros.TransformBroadcaster()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
def __init__(self):
self.leg = kdl.Chain()
self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))
self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))
self.ik_solver = kdl.ChainIkSolverPos_LMA(self.leg)
self.alpha1=45
self.alpha2=-45
self.beta=self.alpha2-self.alpha1
def ikSolve(self):
hulk_kdl_tree = kdl_parser_py.urdf.treeFromFile(
"/home/local/ASURITE/sdsonawa/catkin_ws/src/jackal/urdf/robot.urdf"
)[1]
hulk_transform = hulk_kdl_tree.getChain("dummy_link",
"fake_end_effector_link")
print("Finding Transform Between base_link and end_effector_link")
kdl_fk = KDL.ChainFkSolverPos_recursive(hulk_transform)
kdl_ik = KDL.ChainIkSolverPos_LMA(hulk_transform)
kdl_input = KDL.JntArray(6)
kdl_init = KDL.JntArray(6)
for i in range(5):
kdl_init[i] = self.joints[0, i]
kdl_output = KDL.Frame()
########################################
## offset below provide static transformation between link_0 and camera_link
x_offset = 0.334
y_offset = 0.16
z_offset = 0.59 - 0.35
########################################
initial = True
# if (initial):
# kdl_output.p[0] = 0 + x_offset
# kdl_output.p[1] = -2 + y_offset
# kdl_output.p[2] = 0 + z_offset
# initial = False
# kdl_output.p[0] = self.truss_pose.pose.position.z + x_offset
# kdl_output.p[1] = -self.truss_pose.pose.position.x - y_offset
# kdl_output.p[2] = z_offset - self.truss_pose.pose.position.y
kdl_output.p[0] = 0.3
kdl_output.p[1] = 0.3
kdl_output.p[2] = 0.0
t_0 = rospy.get_time()
kdl_ik.CartToJnt(kdl_init, kdl_output, kdl_input)
t_1 = rospy.get_time()
delta = t_1 - t_0
print(kdl_input[0])
print(kdl_input[1])
print(kdl_input[2])
print(kdl_input[3])
print(kdl_input[4])
self.joint1_pub.publish(self.state_output(kdl_input[0]))
self.joint2_pub.publish(self.state_output(kdl_input[1]))
self.joint3_pub.publish(self.state_output(kdl_input[2]))
self.joint4_pub.publish(self.state_output(kdl_input[3]))
self.joint5_pub.publish(self.state_output(kdl_input[4]))
self.joint6_pub.publish(self.state_output(kdl_input[5]))
def __init__(self):
super(YoubotKDL, self).__init__()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(
self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
def __init__(self):
self.arm = kdl.Chain()
self.arm.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame(kdl.Vector(300, 0, 0))))
self.arm.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame(kdl.Vector(0, -200, 0))))
self.ik_solver = kdl.ChainIkSolverPos_LMA(self.arm)
self.current_joints = kdl.JntArray(self.arm.getNrOfJoints())
self.result_joints = kdl.JntArray(self.arm.getNrOfJoints())
def __init__(self):
self.base_link = 'base_link'
self.ee_link = 'ee_link'
flag, self.tree = kdl_parser.treeFromParam('/robot_description')
self.chain = self.tree.getChain(self.base_link, self.ee_link)
self.num_joints = self.tree.getNrOfJoints()
self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)
self.cam_model = image_geometry.PinholeCameraModel()
self.joint_state = kdl.JntArrayVel(self.num_joints)
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rospy.init_node('ur_eef_vel_controller')
rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
rospy.Subscriber('/rdda_interface/joint_states', JointState,
self.finger_joint_state_cb)
self.joint_vel_cmd_pub = rospy.Publisher(
'/joint_group_vel_controller/command',
Float64MultiArray,
queue_size=10)
self.joint_pos_cmd_pub = rospy.Publisher(
'/scaled_pos_traj_controller/command',
JointTrajectory,
queue_size=10)
self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
Float64,
queue_size=10)
self.switch_controller_cli = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.joint_pos_cli = actionlib.SimpleActionClient(
'/scaled_pos_joint_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
cam_info = rospy.wait_for_message('/camera/depth/camera_info',
CameraInfo,
timeout=10)
self.cam_model.fromCameraInfo(cam_info)
self.bridge = CvBridge() # allows conversion from depth image to array
self.tf_listener = tf.TransformListener()
self.image_offset = 100
self.pos_ctrler_running = False
rospy.sleep(0.5)
def initialize_kinematic_solvers(self):
robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
self._robot = URDF.from_parameter_server(key=robot_description)
self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
# self._base_link = self._robot.get_root()
self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
self._ee_frame = PyKDL.Frame()
self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
def generate_solver(urdf_str: str):
"""Create an FK/IK solvers for each arm (left/right)."""
success, urdf_tree = urdf.treeFromString(urdf_str)
if not success:
raise IOError('Could not parse the URDF!')
chains = {}
fk_solvers = {}
ik_solvers = {}
for side in ['left', 'right']:
chains[side] = urdf_tree.getChain('torso', f'{side}_tip')
fk_solvers[side] = kdl.ChainFkSolverPos_recursive(chains[side])
ik_solvers[side] = kdl.ChainIkSolverPos_LMA(
chains[side],
eps=1e-5,
_maxiter=500,
_eps_joints=1e-15,
)
return chains, fk_solvers, ik_solvers
kdl_model = kdl_model[1]
else:
raise ValueError("Error during the parsing")
#####################
# Chain, FK, and IK #
#####################
#define the kinematic chain
chain_RHand = kdl_model.getChain('DWYTorso', 'RForearm')
#define the Forward Kinematic solver
FK_RHand = kdl.ChainFkSolverPos_recursive(chain_RHand)
#define the Inverse Kinematic solver
IK_RHand = kdl.ChainIkSolverPos_LMA(chain_RHand)
##############
# Gazebo/ROS #
##############
#Define arm
Rarm_joints = ['RShSag', 'RShLat', 'RShYaw', 'RElbj', 'RForearmPlate']
#create publisher
if 'publisher' not in globals():
publisher = ComanPublisher()
#define ROS rate
r = rospy.Rate(ros_rate)
def main():
rospy.init_node('main_test', anonymous=True)
rospy.Subscriber("/iiwa/joint_states", JointState, iiwaStateCallback)
# Initialize publishers for send computation torques.
for i in range(0,7):
torque_controller_pub.append(rospy.Publisher("/iiwa/joint" + str(i+1) + "_torque_controller/command", Float64, queue_size=10))
rate = rospy.Rate(int(1.0/dt))
# Get parameters for kinematic from setup.yaml file
base_link = rospy.get_param("/master/base_link_name")
tool_link = rospy.get_param("/master/tool_link_name")
urdf = rospy.get_param("iiwa_urdf_model")
rospy.loginfo("tip_name: %s" % tool_link)
rospy.loginfo("root_name: %s" % base_link)
data = json.load(open("/workspace/base_ws/src/profi2021_metrics_graber/data/master_tests/track_1_traj.json", "r"))
# Generate kinematic model for orocos_kdl
(ok, tree) = kdl_parser_py.urdf.treeFromString(urdf)
chain = tree.getChain(base_link, tool_link)
L = np.transpose(np.array([[1, 1, 1, 0.01, 0.01, 0.01]]))
iksolverpos = kdl.ChainIkSolverPos_LMA(chain, L)
# Generate dynamic model for orocos_kdl
grav = kdl.Vector(0, 0, -9.82)
dyn_model = kdl.ChainDynParam(chain, grav)
startControllers()
torq_msg = Float64()
counter = 0
u = kdl.JntArray(7)
q_dest = kdl.JntArray(7)
frame_dest = kdl.Frame()
# Setting up initial point
frame_dest.p[0] = data[0]["x"]
frame_dest.p[1] = data[0]["y"]
frame_dest.p[2] = 0.3
# frame_dest.p[0] = 0.3
# frame_dest.p[1] = 0.0
# frame_dest.p[2] = 0.05
frame_dest.M = kdl.Rotation.RotY(3.14)
ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)
while not rospy.is_shutdown():
frame_dest.p[0] = data[int(counter/10)]["x"]
frame_dest.p[1] = data[int(counter/10)]["y"]; # Start controllers
if counter>89*10:
counter=0
ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)
rospy.loginfo("IK solution: %f, %f, %f, %f, %f, %f, %f", q_dest[0], q_dest[1], q_dest[2], q_dest[3], q_dest[4], q_dest[5], q_dest[6])
dyn_model.JntToGravity(q, grav_torques)
rospy.loginfo("Estimation torque on joins: %f, %f, %f, %f, %f, %f, %f\n" % (grav_torques[0], grav_torques[1],grav_torques[2], grav_torques[3], grav_torques[4], grav_torques[5], grav_torques[6]))
for i in range(0,7):
u[i] = KP[i]*(q_dest[i]-q[i]) - KD[i]*dq[i] + grav_torques[i]
torq_msg.data = u[i]
torque_controller_pub[i].publish(torq_msg)
counter+=1
rate.sleep()
def init(self, _chain):
kdlChain = kdl.Chain()
body = _chain.getBaseBody()
# Joint Limits
self.q_min = kdl.JntArray(len(_chain.getJoints()))
self.q_max = kdl.JntArray(len(_chain.getJoints()))
old_pos = kdl.Vector(0, 0, 0)
index = 0
while (len(body.child_joints) != 0): # Last link
# Error if this robot is not a serial robot.
if len(body.child_joints) > 1:
raise InvalidChainError()
# Get Joint Object of type Solver.Joint
joint = _chain.getJoint(body.child_joints[0])
# From Joint (Location of joint on parent body)
trans = kdl.Vector(float(joint.parent_pivot['x']),
float(joint.parent_pivot['y']),
float(joint.parent_pivot['z']))
# From Joint (Location of joint on child body)
trans2 = kdl.Vector(float(joint.child_pivot['x']),
float(joint.child_pivot['y']),
float(joint.child_pivot['z']))
frame = kdl.Frame(trans + old_pos)
old_pos = trans2
kdlJoint = kdl.Joint() # Default Joint Axis
if joint.parent_axis['x']:
kdlJoint = kdl.Joint(kdl.Joint.RotX,
scale=joint.parent_axis['x'])
elif joint.parent_axis['y']:
kdlJoint = kdl.Joint(kdl.Joint.RotY,
scale=joint.parent_axis['y'])
elif joint.parent_axis['z']:
kdlJoint = kdl.Joint(kdl.Joint.RotZ,
scale=joint.parent_axis['z'])
# Add to KDL
kdlChain.addSegment(kdl.Segment(kdlJoint, frame))
# Joint limits
self.q_min[index] = joint.joint_limits['low']
self.q_max[index] = joint.joint_limits['high']
body = _chain.getBody(body.children[0])
index += 1
self.kdlChain = kdlChain
# Initialize Solvers
self.FKSolverPos = kdl.ChainFkSolverPos_recursive(self.kdlChain)
# self.IKVelSolver = kdl.ChainIkSolverVel_pinv_givens(self.kdlChain)
# self.IKPosSolver = kdl.ChainIkSolverPos_NR_JL(self.kdlChain, self.q_min, self.q_max,
# self.FKSolverPos, self.IKVelSolver)
self.IKPosSolver = kdl.ChainIkSolverPos_LMA(self.kdlChain)
jointParameters = kdl.JntArray(chain.getNrOfJoints())
jointParameters[0] = -0.57
jointParameters[1] = 0.12
jointParameters[2] = 0.75
jointParameters[3] = 0.71
# jointParameters[4] = 0
# jointParameters[5] = 0.785
# jointParameters[6] = -0.06
FKSolver = kdl.ChainFkSolverPos_recursive(chain)
FKSolver.JntToCart(jointParameters, output_frame)
print("Final frame :---")
print(output_frame)
IKPosSolver = kdl.ChainIkSolverPos_LMA(chain)
desired_q = kdl.JntArray(chain.getNrOfJoints())
zero_q = kdl.JntArray(chain.getNrOfJoints())
tip_frame = output_frame #kdl.Frame()
IKPosSolver.CartToJnt(zero_q, tip_frame, desired_q)
print("desired_q:--")
print(desired_q)
print("-------------------------")
print("Diff ...")
for i in range(chain.getNrOfJoints()):
print("%3.3f" % (jointParameters[i] - desired_q[i]))
if model[0]:
model = model[1]
else:
raise ValueError("Error during the parsing")
# define the kinematic chain
chain = model.getChain(base_name, end_effector_name)
print("Number of joints in the chain: {}".format(chain.getNrOfJoints()))
# define the FK solver
FK = kdl.ChainFkSolverPos_recursive(chain)
# define the IK Solver
# IKV = kdl.ChainIkSolverVel_pinv(chain)
# IK = kdl.ChainIkSolverPos_NR(chain, FK, IKV)
IK = kdl.ChainIkSolverPos_LMA(
chain) # ,_maxiter=ik_max_iter, _eps_joints=ik_tol)
# desired final cartesian position
Fd = kdl.Frame(kdl.Vector(xd[0], xd[1], xd[2]))
for _ in count():
# get current position in the task/operational space
# x = robot.get_link_positions(link_id, wrt_link_id)
x = robot.get_link_world_positions(link_id)
print("(xd - x) = {}".format(xd - x))
# buffer to put the solution
q_solved = kdl.JntArray(chain.getNrOfJoints())
# initial joint positions
q = robot.get_joint_positions(joint_ids)
