def __init__(self):
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'../assets/urdf/sawyer_intera.urdf')
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self, urdf, srdf, group_name):
self._srdf = SRDF.Group.from_xml_string(srdf)
self._urdf = URDF.urdf.URDF.from_xml_string(urdf)
self._tree = URDF.treeFromUrdfModel(self._urdf)[1]
group = None
for g in self._srdf.groups:
if g.name == group_name:
group = g
if not group:
raise Exception(
"[ServoIk] No group named {} found.".format(group_name))
self.base_link = group.chains[0].base_link
self.tip_link = group.chains[0].tip_link
self._tip_frame = kdl.Frame()
self._chain = self._tree.getChain(self.base_link, self.tip_link)
self._joint_names = self._urdf.joint_map.keys()
self._num_jnts = self._chain.getNrOfJoints()
# Joint limits
self.lower_limits = []
self.upper_limits = []
for jnt in self._urdf.joints:
if jnt.type != 'fixed':
self.lower_limits.append(jnt.limit.lower)
self.upper_limits.append(jnt.limit.upper)
# KDL Solvers
self._fk_p = kdl.ChainFkSolverPos_recursive(self._chain)
self._ik_v = kdl.ChainIkSolverVel_pinv(self._chain)
rospy.loginfo(
"[ServoIk] Started ServoIk for group: {}".format(group_name))
def __init__(
self,
urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'):
'''
Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands
:param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory
'''
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
urdf_relative_path)
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self):
self.num_joints = 7
# Get the URDF (you will need to write your own "load_urdf()" function)
urdf_path = os.path.join(assets_root,
'./kdl/panda_urdf/panda_arm_hand.urdf')
# urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf")
self.urdf_model = URDF.from_xml_string(
open(urdf_path).read().encode(
'ascii')) # encode to ascii solve the error
#self.kdl_tree = treeFromFile(urdf_path)
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self, urdf_param='robot_description'):
self._urdf = URDF.from_parameter_server(urdf_param)
(parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf)
# Check @TODO Exception
if not parse_ok:
rospy.logerr(
'Error parsing URDF from parameter server ({0})'.format(
urdf_param))
else:
rospy.logdebug('Parsing URDF succesful')
self._base_link = self._urdf.get_root()
# @TODO Hardcoded
self._tip_link = 'link_6'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# @TODO Hardcoded
self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6']
self._num_joints = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, urdf_param = 'robot_description'):
self._urdf = URDF.from_parameter_server(urdf_param)
(parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf)
# Check @TODO Exception
if not parse_ok:
rospy.logerr('Error parsing URDF from parameter server ({0})'.format(urdf_param))
else:
rospy.logdebug('Parsing URDF succesful')
self._base_link = self._urdf.get_root()
# @TODO Hardcoded
self._tip_link = 'link_6'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# @TODO Hardcoded
self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6']
self._num_joints = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, urdf, base_link="base_link", end_link="gripper"):
[tree_ok, robot_tree_] = treeFromUrdfModel(urdf)
if not tree_ok:
rospy.logerr("KinematicChain: URDF tree is not parsed properly")
raise Exception("KinematicChain: URDF tree is not parsed properly")
self.robot_tree_ = robot_tree_
self.urdf_model_ = urdf
self.kin_chain_ = robot_tree_.getChain(base_link, end_link)
self.base_link_ = base_link
self.end_link_ = end_link
self.joints_name_ = self.get_joints_name()
self.joints_type_ = self.get_joints_type()
self.links_name_ = self.get_links_name()
self.num_joints_ = len(self.joints_name_)
self.forward_kin_ = kdl.ChainFkSolverPos_recursive(self.kin_chain_)
self.joint_states = [0., 0., 0., 0., 0., 0.]
rospy.Subscriber("/arm/joint_states", JointState,
self.jointStateCallback)
def __init__(self,
name,
base,
ee_link,
namespace=None,
arm_name=None,
compute_fk_for_all=False):
# Joint states
self._joint_angles = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
# Finger chain name
self._name = name
# Namespace
if None in [namespace, arm_name]:
ns = [
item for item in rospy.get_namespace().split('/')
if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called inside the namespace the manipulator namespace'
)
self._namespace = ns[0]
self._arm_name = ns[1]
else:
self._namespace = namespace
self._arm_name = arm_name
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# True if it has to compute the forward kinematics for all frames
self._compute_fk_for_all = compute_fk_for_all
# List of frames for each link
self._frames = dict()
# Load robot description (complete robot, including vehicle, if
# available)
self._robot_description = URDF.from_parameter_server(
key=self._namespace + 'robot_description')
# KDL tree of the whole structure
ok, self._kdl_tree = treeFromUrdfModel(self._robot_description)
# Base link
self._base_link = base
# Tip link
self._tip_link = ee_link
# Read the complete link name, with robot's namespace, if existent
for link in self._robot_description.links:
if self._arm_name not in link.name:
continue
linkname = link.name.split('/')[-1]
if self._base_link == linkname:
self._base_link = link.name
if self._tip_link == linkname:
self._tip_link = link.name
# Get arm chain
self._chain = self._kdl_tree.getChain(self._base_link, self._tip_link)
# Partial tree from base to each link
self._chain_part = dict()
for link in self._robot_description.links:
if link.name in self.link_names:
self._chain_part[link.name] = self._kdl_tree.getChain(
self._base_link, link.name)
# Get joint names from the chain
# Joint names
self._joint_names = list()
for idx in xrange(self._chain.getNrOfSegments()):
joint = self._chain.getSegment(idx).getJoint()
# Not considering fixed joints
if joint.getType() == 0:
name = joint.getName()
self._joint_names.append(name)
self._joint_angles[name] = 0.0
self._joint_velocity[name] = 0.0
self._joint_effort[name] = 0.0
# Jacobian solvers
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._chain)
# Jacobian solver dictionary for partial trees
self._jac_kdl_part = dict()
for link in self._robot_description.links:
if link.name in self.link_names:
self._jac_kdl_part[link.name] = PyKDL.ChainJntToJacSolver(
self._chain_part[link.name])
# Forward position kinematics solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._chain)
# Forward velocity kinematics solvers
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._chain)
# Inverse kinematic solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._chain, self._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
def __init__(self):
# choose between real robot and simulator
self.real_robot = True # real robot
#self.real_robot = False # sim robot
self.wait_for_transform = 0.1
self.listener = tf.TransformListener()
self.listener2 = tf.TransformListener(
) #creating tf.TransformListener object
self.turn_condition = 10
self.turn_penalty = 0
self.double_pi = 2.0 * math.pi
self.Table_flag = False
self.joint_msg = JointState() # to store angles
self.base_vel_msg = Twist() # to store base Twist message
# subscribe to current joint angles
rospy.Subscriber('joint_states',
JointState,
self.JointStateCallback,
queue_size=1)
self.goalpose_msg = PoseStamped() #to store goal pose
self.goalpose_msg1 = PoseStamped()
self.goalpose_msg_odom = PoseStamped()
self.base_to_odom = None
# subscribe to (goal pose) (Type: geometry_msgs/PoseStamped)
rospy.Subscriber('~target_pose',
PoseStamped,
self.ObjectCallback,
queue_size=1)
rospy.Subscriber('/cmd_vel', Twist, self.CmdVelCallback, queue_size=1)
# to trigger the node and start the controller
rospy.Subscriber("~event_in",
String,
self.eventInCallback,
queue_size=1)
self.pub_event_out = rospy.Publisher('~event_out',
String,
queue_size=1)
self.running = False
# to publish joint velocities
self.pub_vel0 = rospy.Publisher('~joint_cmd_vel0',
Float64,
queue_size=1)
self.pub_vel1 = rospy.Publisher('~joint_cmd_vel1',
Float64,
queue_size=1)
self.pub_vel2 = rospy.Publisher('~joint_cmd_vel2',
Float64,
queue_size=1)
self.pub_vel3 = rospy.Publisher('~joint_cmd_vel3',
Float64,
queue_size=1)
self.pub_vel4 = rospy.Publisher('~joint_cmd_vel4',
Float64,
queue_size=1)
self.pub_vel5 = rospy.Publisher('~joint_cmd_vel5',
Float64,
queue_size=1)
self.pub_vel6 = rospy.Publisher('~joint_cmd_vel6',
Float64,
queue_size=1)
self.pub_base_vel_twist = rospy.Publisher('~cmd_vel_pub',
Twist,
queue_size=1)
self.vel_joint0 = Float64() # the msg to publish velocities
self.vel_joint1 = Float64()
self.vel_joint2 = Float64()
self.vel_joint3 = Float64()
self.vel_joint4 = Float64()
self.vel_joint5 = Float64()
self.vel_joint6 = Float64()
self.vel_base = Twist()
self.vel_twist_stamped_jac = TwistStamped()
self.vel_twist_stamped_lin = TwistStamped()
self.vel_twist_stamped_now = TwistStamped()
self.vel_twist_stamped_opt = TwistStamped()
self.goalpose_received = False # flag to indicate that a goal pose msg was received
self.joint_received = False # flag to indicate that a joint statese msg was received
self.base_vel_received = False # flag to indicate that a base vel msg was received
self.translation = None
self.rotation = None
self.phi = None
self.current_phi_vel = None
self.current_base_vel = [0, 0, 0] #x,y,theta
self.vel = [0, 0, 0] #ee desired linear velocity
self.ang_vel = [0, 0, 0] #roll,pitch,yaw, ee desired angular velocity
self.x1 = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
] #initial conditions for optimization - servos velocities phi' (7x1) + base = (10x1)
self.robot = URDF.from_parameter_server('robot_description')
self.tree = treeFromUrdfModel(self.robot)[1] #tupel error before
#self.joint_names = self.getJointsNames()
self.joint_names = ['left_arm_joint0', 'left_arm_joint1', 'left_arm_joint2',\
'left_arm_joint3', 'left_arm_joint4', 'left_arm_joint5', 'left_arm_joint6']
self.chain = self.tree.getChain("base_link",
"left_arm_virtual_endeffector")
self.num_joints = self.chain.getNrOfJoints()
self.jac_kdl = PyKDL.ChainJntToJacSolver(self.chain)
self.b = (
-0.2, 0.2
) #bounds for arm servos - veocities limits same for all 7 joints
self.base = (-0.3, 0.3) #and 3 for base
self.bnds = (self.b, self.b, self.b, self.b, self.b, self.b, self.b,
self.base, self.base, self.b)
self.con1 = {'type': 'eq', 'fun': self.constraint1}
self.con2 = {'type': 'ineq', 'fun': self.constraint_min}
self.con3 = {'type': 'ineq', 'fun': self.constraint_max}
self.cons = [self.con2, self.con3] #self.con1,
self.p_prim1 = None
self.solex = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# flag to know when to start, based upon required tf msg
self.ready_to_start = False
# query weight params from param server
self.base_x_weight = rospy.get_param('~base_x_weight', 4)
self.base_y_weight = rospy.get_param('~base_y_weight', 4)
self.base_theta_weight = rospy.get_param('~base_theta_weight', 8)
base_link = 'panda_link0'
end_link = 'panda_link7'
try:
rospy.init_node('jacobian_planner')
load_robot_description()
time.sleep(2)
print("Get urdf model from parameter server")
model = URDF.from_parameter_server()
#print model
print("Loading kdl tree using urdf from parameter server")
ret, kdltree = urdf.treeFromUrdfModel(model, quiet=False)
# Get the whole kinematic chain from kdl tree
chain = kdltree.getChain(base_link, end_link)
# Initialize kdl solver for jacobian calculation
solver = kdl.ChainJntToJacSolver(chain)
jnt_array = kdl.JntArray(chain.getNrOfSegments())
# Get joints from the chain
#for i in range(chain.getNrOfSegments()):
# print chain.getSegment(i).getJoint()
jac = kdl.Jacobian(chain.getNrOfSegments())
# print(dir(kdl))