def __init__(self, base_link=None, end_link=None):
self._robot = kdl_parser_py.urdf.urdf.URDF.from_parameter_server(
'robot_description')
(ok,
self._kdl_tree) = kdl_parser_py.urdf.treeFromUrdfModel(self._robot)
self._base_link = self._robot.get_root()
self._tip_link = end_link
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
#self.joint_names = [joint.name for joint in self._robot.joints if joint.type!='fixed']
self.joint_names = [
self._arm_chain.getSegment(i).getJoint().getName()
for i in range(self._arm_chain.getNrOfSegments())
]
self._num_jnts = len(self.joint_names)
# Store joint information for future use
self.get_joint_information()
# 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_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, 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):
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, robot, ee_link=None):
rospack = rospkg.RosPack()
pykdl_dir = rospack.get_path('ur_pykdl')
TREE_PATH = pykdl_dir + '/urdf/' + robot + '.urdf'
self._ur = URDF.from_xml_file(TREE_PATH)
self._kdl_tree = kdl_tree_from_urdf_model(self._ur)
self._base_link = BASE_LINK
ee_link = EE_LINK if ee_link is None else ee_link
self._tip_link = ee_link
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# UR Interface Limb Instances
self._joint_names = JOINT_ORDER
self._num_jnts = 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, limb):
self._kuka = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._kuka)
self._base_link = self._kuka.get_root()
# self._tip_link = limb + '_gripper'
self._tip_link = 'tool0'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Kuka Interface Limb Instances
# self._limb_interface = baxter_interface.Limb(limb)
# self._joint_names = self._limb_interface.joint_names()
self._joint_names = [
'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5',
'joint_a6'
]
self._num_jnts = 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, limb, description=None):
if description is None:
self._franka = URDF.from_parameter_server(key='robot_description')
else:
self._franka = URDF.from_xml_file(description)
self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
self._base_link = self._franka.get_root()
self._tip_link = limb.name + ('_hand'
if limb.has_gripper else '_link8')
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._limb_interface = limb
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = 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):
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, limb='right'):
self._sawyer = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
self._base_link = self._sawyer.get_root()
# RUDI: LETS GET RID OF INTERA
#self._tip_link = limb + '_hand'
self._tip_link = 'right_gripper_tip'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# RUDI: LETS GET RID OF INTERA
# Sawyer Interface Limb Instances
# self._limb_interface = intera_interface.Limb(limb)
#self._joint_names = self._limb_interface.joint_names()
self._joint_names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"]
self._num_jnts = 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, limb):
self._pr2 = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._pr2)
self._base_link = self._pr2.get_root()
# self._tip_link = limb + '_gripper'
self._tip_link = limb + '_wrist_roll_link'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Baxter Interface Limb Instances
# self._limb_interface = baxter_interface.Limb(limb)
# self._joint_names = self._limb_interface.joint_names()
self._joint_names = [
limb + '_shoulder_pan_joint', limb + '_shoulder_lift_joint',
limb + '_upper_arm_roll_joint', limb + '_elbow_flex_joint',
limb + '_forearm_roll_joint', limb + '_wrist_flex_joint',
limb + '_wrist_roll_joint'
]
self._num_jnts = 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, limb):
self._sawyer = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
self._base_link = self._sawyer.get_root()
print "BASE LINK:", self._base_link
self._tip_link = limb + '_hand'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Sawyer Interface Limb Instances
self._limb_interface = intera_interface.Limb(limb)
self._joint_names = self._limb_interface.joint_names()
self._num_jnts = 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(0.0, 0.0, -9.81))
def __init__(self, urdf, base_link, end_link, kdl_tree=None):
if kdl_tree is None:
kdl_tree = kdl_tree_from_urdf_model(urdf)
self.tree = kdl_tree
self.urdf = urdf
base_link = base_link.split("/")[-1] # for dealing with tf convention
end_link = end_link.split("/")[-1] # for dealing with tf convention
self.chain = kdl_tree.getChain(base_link, end_link)
self.base_link = base_link
self.end_link = end_link
# record joint information in easy-to-use lists
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_safety_lower = []
self.joint_safety_upper = []
self.joint_types = []
for jnt_name in self.get_joint_names():
jnt = urdf.joints[jnt_name]
if jnt.limits is not None:
self.joint_limits_lower.append(jnt.limits.lower)
self.joint_limits_upper.append(jnt.limits.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
if jnt.safety is not None:
self.joint_safety_lower.append(jnt.safety.lower)
self.joint_safety_upper.append(jnt.safety.upper)
elif jnt.limits is not None:
self.joint_safety_lower.append(jnt.limits.lower)
self.joint_safety_upper.append(jnt.limits.upper)
else:
self.joint_safety_lower.append(None)
self.joint_safety_upper.append(None)
self.joint_types.append(jnt.joint_type)
def replace_none(x, v):
if x is None:
return v
return x
self.joint_limits_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_limits_lower])
self.joint_limits_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_limits_upper])
self.joint_safety_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_safety_lower])
self.joint_safety_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_safety_upper])
self.joint_types = np.array(self.joint_types)
self.num_joints = len(self.get_joint_names())
self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain)
self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain)
self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl,
self._ik_v_kdl)
self._jac_kdl = kdl.ChainJntToJacSolver(self.chain)
self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector.Zero())
def __init__(self):
ns = [
item for item in rospy.get_namespace().split('/') if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called in the manipulator namespace')
self._namespace = ns[0]
self._arm_name = ns[1]
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# The arm interface loads the parameters from the URDF file and
# parameter server and initializes the KDL tree
self._arm_interface = ArmInterface()
self._base_link = self._arm_interface.base_link
self._tip_link = self._arm_interface.tip_link
self._tip_frame = PyKDL.Frame()
# KDL Solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(
self._arm_interface.chain, self._arm_interface._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain,
PyKDL.Vector.Zero())
# Add a callback function to calculate the end effector's state by each
# update of the joint state
self._arm_interface.add_callback('joint_states',
self.on_update_endeffector_state)
# Publish the current manipulability index at each update of the joint
# states
# self._arm_interface.add_callback('joint_states',
# self.publish_man_index)
# Publish the end effector state
self._endeffector_state_pub = rospy.Publisher('endpoint_state',
EndPointState,
queue_size=1)
# Publish the manipulability index
self._man_index_pub = rospy.Publisher('man_index',
Float64,
queue_size=1)
self._services = dict()
# Provide the service to calculate the inverse kinematics using the KDL solver
self._services['ik'] = rospy.Service('ik_solver', SolveIK,
self.solve_ik)
def _set_solvers(self):
"""Initialize the solvers according to the stored base chain and
tool"""
self._complete_chain = kdl.Chain(self._base_chain)
if self._tool_segment is not None:
if isinstance(self._tool_segment, kdl.Segment):
self._complete_chain.addSegment(self._tool_segment)
else:
raise Exception("Tool is not None, Chain or Segment")
self._fk_solver_pos = kdl.ChainFkSolverPos_recursive(
self._complete_chain)
self._tv_solver = kdl.ChainIkSolverVel_pinv(self._complete_chain)
self._ik_solver_pos = kdl.ChainIkSolverPos_NR(self._complete_chain,
self._fk_solver_pos,
self._tv_solver)
def __init__(self, root_link=None, tip_link="left_arm_7_link"):
self._urdf = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._urdf)
self._base_link = self._urdf.get_root(
) if root_link is None else root_link
self._tip_link = tip_link
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._joint_names = self._urdf.get_chain(self._base_link,
self._tip_link,
links=False,
fixed=False)
self._num_jnts = len(self._joint_names)
# KDL
self._fk_p_kdl = PyKDL.ChainFkSolverPos_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)
# trac_ik
urdf_str = rospy.get_param('/robot_description')
self.trac_ik_solver = IK(self._base_link,
self._tip_link,
urdf_string=urdf_str)
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_types = []
for jnt_name in self._joint_names:
jnt = self._urdf.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
self.joint_types.append(jnt.type)
self._default_seed = [
-0.3723750412464142, 0.02747996523976326, -0.7221865057945251,
-1.69843590259552, 0.11076358705759048, 0.5450965166091919,
0.45358774065971375
] # home_pos
def cargarURDF(self):
# filename = '/home/kaiser/WS_ROS/catkin_ws/src/abb_experimental-indigo-devel/abb_irb120_gazebo/urdf/modelo_exportado.urdf'
# filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/IRB120_URDF_v2/robots/IRB120_URDF_v2.URDF'
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/irb120_peq/irb120_peq.urdf'
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
self.chain = tree.getChain("base_link", "link_6")
print self.chain.getNrOfSegments()
self.solverCinematicaDirecta = PyKDL.ChainFkSolverPos_recursive(
self.chain)
self.vik = PyKDL.ChainIkSolverVel_pinv(self.chain)
self.ik = PyKDL.ChainIkSolverPos_NR(self.chain,
self.solverCinematicaDirecta,
self.vik)
self.actualizar_IK([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
def __init__(self,
limb,
ee_frame_name="panda_link8",
additional_segment_config=None,
description=None):
if description is None:
self._franka = URDF.from_parameter_server(key='robot_description')
else:
self._franka = URDF.from_xml_file(description)
self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
if additional_segment_config is not None:
for c in additional_segment_config:
q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist()
kdl_origin_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w),
PyKDL.Vector(*(c["origin_pos"].tolist())))
kdl_sgm = PyKDL.Segment(c["child_name"],
PyKDL.Joint(c["joint_name"]),
kdl_origin_frame,
PyKDL.RigidBodyInertia())
self._kdl_tree.addSegment(kdl_sgm, c["parent_name"])
self._base_link = self._franka.get_root()
self._tip_link = ee_frame_name
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._limb_interface = limb
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = 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 main():
filename = None
if (sys.argv > 1):
filename = 'uthai_description/urdf/uthai.urdf'
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("base_footprint","r_foot_ft_link")
fksolver = kdl.ChainFkSolverPos_recursive(chain)
ikvelsolver = kdl.ChainIkSolverVel_pinv(chain)
iksolver = kdl.ChainIkSolverPos_NR(chain,fksolver,ikvelsolver)
q_init = kdl.JntArray(6)
q = kdl.JntArray(6)
F_dest = kdl.Frame(kdl.Rotation.RPY(0,0,0),kdl.Vector(0.0,0,0))
status = iksolver.CartToJnt(q_init,F_dest,q)
print("Status : ",status)
print(q)
def create_chain(self, ee_frame_name):
self._tip_link = ee_frame_name
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = 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, rospy_init=False):
if rospy_init:
rospy.init_node('baxter_kinematics')
# KDL chain from URDF
self._urdf = URDF.from_parameter_server(key='robot_description')
self._base_link = 'left_arm_mount' #self._urdf.get_root()
self._tip_link = 'left_wrist'
self._kdl_tree = kdl_tree_from_urdf_model(self._urdf)
self._kdl_chain = self._kdl_tree.getChain(self._base_link, self._tip_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._kdl_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._kdl_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._kdl_chain, self._fk_p_kdl, self._ik_v_kdl)
# Baxter Interface Limb Instances
self._limb = baxter_interface.Limb('left')
self._joint_names = self._limb.joint_names()
self._joint_names = [jn for jn in self._joint_names] # left_e0 is fixed
self._num_jnts = len(self._joint_names)
def configure(self, tf_base_link_name, tf_end_link_name):
self.tf_base_link_name = tf_base_link_name
self.tf_end_link_name = tf_end_link_name
#Variables holding the joint information
self.robot = URDF.from_parameter_server()
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(self.tf_base_link_name,
self.tf_end_link_name)
self.kdl_kin = KDLKinematics(self.robot, self.tf_base_link_name,
self.tf_end_link_name)
self.arm_joint_names = self.kdl_kin.get_joint_names()
self.jnt_pos = kdl.JntArray(self.chain.getNrOfJoints())
self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.ikv_solver.setLambda(0.0001)
#self.ikv_solver.setWeightTS([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,0,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]])
self.ik_solver = kdl.ChainIkSolverPos_NR(self.chain, self.fk_solver,
self.ikv_solver)
def __init__(self):
pi4 = 0.7853
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_robot_xacro4_cambio.urdf'
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")
print cadena_der_up_down.getNrOfSegments()
fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)
vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)
vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)
vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)
vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)
nj_izq = cadena_der_up_down.getNrOfJoints()
posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_der_up_down[0] = 0
posicionInicial_der_up_down[1] = 0.3
posicionInicial_der_up_down[2] = -0.3
posicionInicial_der_up_down[3] = 0.6
posicionInicial_der_up_down[4] = -0.3
posicionInicial_der_up_down[5] = -0.3
nj_izq = cadena_izq_up_down.getNrOfJoints()
posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_izq_up_down[0] = 0
posicionInicial_izq_up_down[1] = 0.3
posicionInicial_izq_up_down[2] = -0.3
posicionInicial_izq_up_down[3] = 0.6
posicionInicial_izq_up_down[4] = -0.3
posicionInicial_izq_up_down[5] = -0.3
nj_izq = cadena_der_down_up.getNrOfJoints()
posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_der_down_up[5] = 0
posicionInicial_der_down_up[4] = 0.3
posicionInicial_der_down_up[3] = -0.3
posicionInicial_der_down_up[2] = 0.6
posicionInicial_der_down_up[1] = -0.3
posicionInicial_der_down_up[0] = -0.3
nj_izq = cadena_izq_down_up.getNrOfJoints()
posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_izq_down_up[5] = 0
posicionInicial_izq_down_up[4] = 0.3
posicionInicial_izq_down_up[3] = -0.3
posicionInicial_izq_down_up[2] = 0.6
posicionInicial_izq_down_up[1] = -0.3
posicionInicial_izq_down_up[0] = -0.3
print "Forward kinematics"
finalFrame_izq_up_down = PyKDL.Frame()
finalFrame_izq_down_up = PyKDL.Frame()
finalFrame_der_up_down = PyKDL.Frame()
finalFrame_der_down_up = PyKDL.Frame()
print "Rotational Matrix of the final Frame: "
print finalFrame_izq_up_down.M
print "End-effector position: ", finalFrame_izq_up_down.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = ['cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint','cilinder_blue_box2_der_joint',
'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint','cilinder_blue_box6_der_joint',
'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint','cilinder_blue_box2_izq_joint',
'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint','cilinder_blue_box6_izq_joint']
piernas_goal0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame = finalFrame_izq_up_down
desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
print "Inverse Kinematics"
fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame_der_up_down
desiredFrame.p[0] = finalFrame_der_up_down.p[0]
desiredFrame.p[1] = finalFrame_der_up_down.p[1]
desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
print "Desired Position: ", desiredFrame.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal0[6] = q_out_izq_up_down[0]
piernas_goal0[7] = q_out_izq_up_down[1]
piernas_goal0[8] = q_out_izq_up_down[2]
piernas_goal0[9] = q_out_izq_up_down[3]
piernas_goal0[10] = q_out_izq_up_down[4]
piernas_goal0[11] = q_out_izq_up_down[5]
piernas_goal0[0] = q_out_der_up_down[0]
piernas_goal0[1] = q_out_der_up_down[1]
piernas_goal0[2] = q_out_der_up_down[2]
piernas_goal0[3] = q_out_der_up_down[3]
piernas_goal0[4] = q_out_der_up_down[4]
piernas_goal0[5] = q_out_der_up_down[5]
#121212122121212121212121212121212121212121212
piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#q_out_izq_up_down[0] -= pi4
piernas_goal12[6] = q_out_izq_up_down[0]-2*pi4
piernas_goal12[7] = q_out_izq_up_down[1]
piernas_goal12[8] = q_out_izq_up_down[2]
piernas_goal12[9] = q_out_izq_up_down[3]
piernas_goal12[10] = q_out_izq_up_down[4]
piernas_goal12[11] = q_out_izq_up_down[5]
piernas_goal12[0] = 0
piernas_goal12[1] = 0
piernas_goal12[2] = -0.7
piernas_goal12[3] = 1.4
piernas_goal12[4] = 0
piernas_goal12[5] = -0.7
#########################################################
piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal2[6] = q_out_izq_up_down[0]-2*pi4
piernas_goal2[7] = q_out_izq_up_down[1]-0.3-0.3
piernas_goal2[8] = q_out_izq_up_down[2]-0.3
piernas_goal2[9] = q_out_izq_up_down[3]+0.6
piernas_goal2[10] = q_out_izq_up_down[4]+0.3+0.3
piernas_goal2[11] = q_out_izq_up_down[5] -0.3
piernas_goal2[0] = 0#-pi4
piernas_goal2[1] = -0.25
piernas_goal2[2] = -0.3
piernas_goal2[3] = 0.6
piernas_goal2[4] = 0.25
piernas_goal2[5] = -0.3
##################################################
piernas_goal3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal3[6] = q_out_izq_up_down[0] +0.0
piernas_goal3[7] = q_out_izq_up_down[1] - 0.3
piernas_goal3[8] = q_out_izq_up_down[2] - 0.3
piernas_goal3[9] = q_out_izq_up_down[3] + 0.6
piernas_goal3[10] = q_out_izq_up_down[4] + 0.3
piernas_goal3[11] = q_out_izq_up_down[5] - 0.3
piernas_goal3[0] = -2*pi4
piernas_goal3[1] = -0.25
piernas_goal3[2] = -0.3
piernas_goal3[3] = 0.6
piernas_goal3[4] = 0.25
piernas_goal3[5] = -0.3
##################################################
piernas_goal4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
piernas_goal4[6] = q_out_izq_up_down[0] +0.0
piernas_goal4[7] = q_out_izq_up_down[1] - 0.3 +0.3
piernas_goal4[8] = q_out_izq_up_down[2] - 0.3 +0.3
piernas_goal4[9] = q_out_izq_up_down[3] + 0.6 -0.6
piernas_goal4[10] = q_out_izq_up_down[4] + 0.3 -0.3
piernas_goal4[11] = q_out_izq_up_down[5] - 0.3 +0.3
piernas_goal4[0] = -2*pi4
piernas_goal4[1] = -0.25
piernas_goal4[2] = -0.7
piernas_goal4[3] = 1.4
piernas_goal4[4] = 0.25
piernas_goal4[5] = -0.7
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
#/piernas/piernas_controller/follow_joint_trajectory
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = piernas_goal0
arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[1].positions = piernas_goal12
arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[1].time_from_start = rospy.Duration(6.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[2].positions = piernas_goal2
arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[2].time_from_start = rospy.Duration(9.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[3].positions = piernas_goal3
arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[3].time_from_start = rospy.Duration(12.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[4].positions = piernas_goal4
arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[4].time_from_start = rospy.Duration(15.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[5].positions = piernas_goal12
arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[5].time_from_start = rospy.Duration(18.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[6].positions = piernas_goal2
arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[6].time_from_start = rospy.Duration(21.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[7].positions = piernas_goal3
arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[7].time_from_start = rospy.Duration(24)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[8].positions = piernas_goal4
arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[8].time_from_start = rospy.Duration(27)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[9].positions = piernas_goal12
arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[9].time_from_start = rospy.Duration(30.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[10].positions = piernas_goal2
arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[10].time_from_start = rospy.Duration(33.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[11].positions = piernas_goal3
arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[11].time_from_start = rospy.Duration(36.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[12].positions = piernas_goal4
arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[12].time_from_start = rospy.Duration(39.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[13].positions = piernas_goal12
arm_trajectory.points[13].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[13].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[13].time_from_start = rospy.Duration(42.0)
'''arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[14].positions = piernas_goal4
arm_trajectory.points[14].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[14].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[14].time_from_start = rospy.Duration(32.0)'''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
piernas_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
piernas_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
piernas_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
arm_client.send_goal(piernas_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result()
print arm_client.get_result()
rospy.loginfo('...done')
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
datos_articulaciones = open('datos_articulaciones.pkl', 'wb')
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")
print cadena_der_up_down.getNrOfSegments()
fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)
vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)
vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)
vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)
vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)
nj_izq = cadena_der_up_down.getNrOfJoints()
posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_der_up_down[0] = 0.3
posicionInicial_der_up_down[1] = -0.3
posicionInicial_der_up_down[2] = 0
posicionInicial_der_up_down[3] = 0.6
posicionInicial_der_up_down[4] = -0.3
posicionInicial_der_up_down[5] = -0.3
nj_izq = cadena_izq_up_down.getNrOfJoints()
posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_izq_up_down[0] = 0.3
posicionInicial_izq_up_down[1] = -0.3
posicionInicial_izq_up_down[2] = 0.0
posicionInicial_izq_up_down[3] = 0.6
posicionInicial_izq_up_down[4] = -0.3
posicionInicial_izq_up_down[5] = -0.3
nj_izq = cadena_der_down_up.getNrOfJoints()
posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_der_down_up[5] = 0.3
posicionInicial_der_down_up[4] = -0.3
posicionInicial_der_down_up[3] = 0.0
posicionInicial_der_down_up[2] = 0.6
posicionInicial_der_down_up[1] = -0.3
posicionInicial_der_down_up[0] = -0.3
nj_izq = cadena_izq_down_up.getNrOfJoints()
posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_izq_down_up[5] = 0.3
posicionInicial_izq_down_up[4] = -0.3
posicionInicial_izq_down_up[3] = 0.0
posicionInicial_izq_down_up[2] = 0.6
posicionInicial_izq_down_up[1] = -0.3
posicionInicial_izq_down_up[0] = -0.3
print "Forward kinematics"
finalFrame_izq_up_down = PyKDL.Frame()
finalFrame_izq_down_up = PyKDL.Frame()
finalFrame_der_up_down = PyKDL.Frame()
finalFrame_der_down_up = PyKDL.Frame()
print "Rotational Matrix of the final Frame: "
print finalFrame_izq_up_down.M
print "End-effector position: ", finalFrame_izq_up_down.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint',
'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint',
'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ]
piernas_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame = finalFrame_izq_up_down
desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
piernas_goal[6] = q_out_izq_up_down[0]
piernas_goal[7] = q_out_izq_up_down[1]
piernas_goal[8] = q_out_izq_up_down[2]
piernas_goal[9] = q_out_izq_up_down[3]
piernas_goal[10] = q_out_izq_up_down[4]
piernas_goal[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame_der_up_down
desiredFrame.p[0] = finalFrame_der_up_down.p[0]
desiredFrame.p[1] = finalFrame_der_up_down.p[1]
desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
print "Desired Position: ", desiredFrame.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal[0] = q_out_der_up_down[0]
piernas_goal[1] = q_out_der_up_down[1]
piernas_goal[2] = q_out_der_up_down[2]
piernas_goal[3] = q_out_der_up_down[3]
piernas_goal[4] = q_out_der_up_down[4]
piernas_goal[5] = q_out_der_up_down[5]
#121212122121212121212121212121212121212121212
piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame.p[0] = desiredFrame.p[0]
desiredFrame.p[1] = desiredFrame.p[1]
desiredFrame.p[2] = desiredFrame.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
piernas_goal12[6] = q_out_izq_up_down[0]
piernas_goal12[7] = q_out_izq_up_down[1]
piernas_goal12[8] = q_out_izq_up_down[2]
piernas_goal12[9] = q_out_izq_up_down[3]
piernas_goal12[10] = q_out_izq_up_down[4]
piernas_goal12[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
desiredFrame0 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame0.p[0] = desiredFrame0.p[0]
desiredFrame0.p[1] = desiredFrame0.p[1] -0.07
desiredFrame0.p[2] = desiredFrame0.p[2]
print "Desired Position: ", desiredFrame0.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal12[0] = q_out_der_up_down[0]
piernas_goal12[1] = q_out_der_up_down[1]
piernas_goal12[2] = q_out_der_up_down[2]
piernas_goal12[3] = q_out_der_up_down[3]
piernas_goal12[4] = q_out_der_up_down[4]
piernas_goal12[5] = q_out_der_up_down[5]
# 222222222222222222222222222222222222222222
piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame2 = PyKDL.Frame()
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2)
q_init_izq_down_up = posicionInicial_izq_down_up # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame2.p[0] = desiredFrame2.p[0] -0.06 #0.05
desiredFrame2.p[1] = desiredFrame2.p[1] -0.06#0.06
desiredFrame2.p[2] = desiredFrame2.p[2] -0.01 #0.02
print "Desired Position2222aaa: ", desiredFrame2.p
#print desiredFrame2.M
q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up)
print "Output angles in rads2222: ", q_out_izq_down_up
piernas_goal2[6] = q_out_izq_down_up[5]#+0.1
piernas_goal2[7] = q_out_izq_down_up[4]#-0.05
piernas_goal2[8] = q_out_izq_down_up[3]
piernas_goal2[9] = q_out_izq_down_up[2]
piernas_goal2[10] = q_out_izq_down_up[1]
piernas_goal2[11] = q_out_izq_down_up[0]
#q_out_izq_down_up[4] -=0.1
print "Inverse Kinematics"
q_init_der_down_up = PyKDL.JntArray(6)
q_init_der_down_up[0] = q_out_der_up_down[5] # PROBLEMAS CON LA ASIGNACION
q_init_der_down_up[1] = q_out_der_up_down[4]
q_init_der_down_up[2] = q_out_der_up_down[3]
q_init_der_down_up[3] = q_out_der_up_down[2]
q_init_der_down_up[4] = q_out_der_up_down[1]
q_init_der_down_up[5] = q_out_der_up_down[0]+0.08
fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame3 = finalFrame_der_down_up
desiredFrame3.p[0] = finalFrame_der_down_up.p[0]- 0.05
desiredFrame3.p[1] = finalFrame_der_down_up.p[1]- 0.04
desiredFrame3.p[2] = finalFrame_der_down_up.p[2]-0.02
print "Desired Position2222der: ", desiredFrame3.p
q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up)
print "Output angles in rads22222der: ", q_out_der_down_up
print "VALOR", q_out_der_up_down[5]
piernas_goal2[0] = -0.3
piernas_goal2[1] = -0.3
piernas_goal2[2] = 0
piernas_goal2[3] = 0.6
piernas_goal2[4] = 0.3
piernas_goal2[5] = -0.3
# 333333333333333333333333333333333333333333333333
piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
desiredFrame4 = PyKDL.Frame()
fksolver_izq_down_up.JntToCart(q_out_izq_down_up,desiredFrame4)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame4.p[0] = desiredFrame4.p[0]
desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02
desiredFrame4.p[2] = desiredFrame4.p[2]
ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up)
q_init_izq_up_down[0] = q_out_izq_down_up[5]
q_init_izq_up_down[1] = q_out_izq_down_up[4]
q_init_izq_up_down[2] = q_out_izq_down_up[3]
q_init_izq_up_down[3] = q_out_izq_down_up[2]
q_init_izq_up_down[4] = q_out_izq_down_up[1]
q_init_izq_up_down[5] = q_out_izq_down_up[0]
desiredFrame5 = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_init_izq_up_down,desiredFrame5)
desiredFrame5.p[0] = desiredFrame5.p[0]
desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1
desiredFrame5.p[2] = desiredFrame5.p[2]+0.01
print "Desired Position: ", desiredFrame5.p
q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2)
print "Output angles in rads: ", q_out_izq_up_down2
piernas_goal3[6] = q_out_izq_up_down2[0]
piernas_goal3[7] = q_out_izq_up_down2[1]
piernas_goal3[8] = q_out_izq_up_down2[2]
piernas_goal3[9] = q_out_izq_up_down2[3]
piernas_goal3[10] = q_out_izq_up_down2[4]#+0.15
piernas_goal3[11] = q_out_izq_up_down2[5]-0.08
print "Inverse Kinematics"
piernas_goal3[0] = -0.3
piernas_goal3[1] = -0.3
piernas_goal3[2] = 0
piernas_goal3[3] = 0.6
piernas_goal3[4] = 0.3
piernas_goal3[5] = -0.3
# 121212122121212121212121212121212121212121212
piernas_goal121 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame6 = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = q_out_izq_up_down2 # initial angles #CUIDADO
desiredFrame6.p[0] = desiredFrame6.p[0]
desiredFrame6.p[1] = desiredFrame6.p[1]-0.05
desiredFrame6.p[2] = desiredFrame6.p[2]#+0.01
print "Desired Position: ", desiredFrame6.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down)
print "Output angles in rads_izq_121: ", q_out_izq_up_down
piernas_goal121[6] = q_out_izq_up_down[0]
piernas_goal121[7] = q_out_izq_up_down[1]
piernas_goal121[8] = q_out_izq_up_down[2]
piernas_goal121[9] = q_out_izq_up_down[3]
piernas_goal121[10] = q_out_izq_up_down[4]
piernas_goal121[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
desiredFrame06 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = q_out_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame06.p[0] = desiredFrame06.p[0]
desiredFrame06.p[1] = desiredFrame06.p[1]
desiredFrame06.p[2] = desiredFrame06.p[2]
print "Desired Position: ", desiredFrame06.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
q_out_der_up_down21 = PyKDL.JntArray(6)
q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3 ]
piernas_goal121[0] = q_out_der_up_down21[0]
piernas_goal121[1] = q_out_der_up_down21[1]
piernas_goal121[2] = q_out_der_up_down21[2]
piernas_goal121[3] = q_out_der_up_down21[3]
piernas_goal121[4] = q_out_der_up_down21[4]
piernas_goal121[5] = q_out_der_up_down21[5]
# 55555555555555555555555555555555555555555
piernas_goal25 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame7= PyKDL.Frame()
q_init_izq_down_up3 = PyKDL.JntArray(6)
q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1
q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1
q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1
q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1
q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1
q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1
fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05#0.03 # PROBAR A PONERLO MAYOR
desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06#0.04
desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005
print "Desired Position2222: ", desiredFrame7.p
q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5)
print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5
piernas_goal25[6] = q_out_izq_down_up5[5]
piernas_goal25[7] = q_out_izq_down_up5[4]
piernas_goal25[8] = q_out_izq_down_up5[3]
piernas_goal25[9] = q_out_izq_down_up5[2]
piernas_goal25[10] = q_out_izq_down_up5[1]-0.05
piernas_goal25[11] = q_out_izq_down_up5[0]+0.05
print "Inverse Kinematics"
q_init_der_down_up31 = PyKDL.JntArray(6)
q_init_der_down_up31[0] = q_out_der_up_down21[5] *1 # PROBLEMAS CON LA ASIGNACION
q_init_der_down_up31[1] = q_out_der_up_down21[4] *1
q_init_der_down_up31[2] = q_out_der_up_down21[3] *1
q_init_der_down_up31[3] = q_out_der_up_down21[2] *1
q_init_der_down_up31[4] = q_out_der_up_down21[1] *1
q_init_der_down_up31[5] = q_out_der_up_down21[0] *1
desiredFrame7 = PyKDL.Frame()
fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05
desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06
desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02
print "Desired Position2222der: ", desiredFrame7.p
q_out_der_down_up71 = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71)
print "Output angles in rads22222der: ", q_out_der_down_up71
piernas_goal25[0] = q_out_der_down_up71[5]
piernas_goal25[1] = q_out_der_down_up71[4]
piernas_goal25[2] = q_out_der_down_up71[3]
piernas_goal25[3] = q_out_der_down_up71[2]
piernas_goal25[4] = q_out_der_down_up71[1]
piernas_goal25[5] = q_out_der_down_up71[0]
# 333333333333333333333333333333333333333333333333
piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
desiredFrame441 = PyKDL.Frame()
fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame441.p[0] = desiredFrame441.p[0]
desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02
desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015
ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71)
q_init_der_up_down[0] = q_out_der_down_up71[5]
q_init_der_up_down[1] = q_out_der_down_up71[4]
q_init_der_up_down[2] = q_out_der_down_up71[3]
q_init_der_up_down[3] = q_out_der_down_up71[2]
q_init_der_up_down[4] = q_out_der_down_up71[1]
q_init_der_up_down[5] = q_out_der_down_up71[0]
desiredFrame541 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541)
desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03
desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1
desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada
print "Desired Position: ", desiredFrame541.p
q_out_der_up_down241 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241)# con desiredFrame5 va
print "Output angles in radsaaaaa: ", q_out_der_up_down241
print "Inverse Kinematics"
piernas_goal341[6] = 0.3
piernas_goal341[7] = -0.3
piernas_goal341[8] = 0
piernas_goal341[9] = 0.6
piernas_goal341[10] = -0.3
piernas_goal341[11] = -0.3
piernas_goal341[0] = q_out_der_up_down241[0]#+0.1
piernas_goal341[1] = q_out_der_up_down241[1]
piernas_goal341[2] = q_out_der_up_down241[2]
piernas_goal341[3] = q_out_der_up_down241[3]
piernas_goal341[4] = q_out_der_up_down241[4]#-0.1
piernas_goal341[5] = q_out_der_up_down241[5]#-0.01
pickle.dump(piernas_goal, datos_articulaciones, -1)
pickle.dump(piernas_goal12, datos_articulaciones, -1)
pickle.dump(piernas_goal2, datos_articulaciones, -1)
pickle.dump(piernas_goal3, datos_articulaciones, -1)
pickle.dump(piernas_goal121, datos_articulaciones, -1)
pickle.dump(piernas_goal25, datos_articulaciones, -1)
pickle.dump(piernas_goal341, datos_articulaciones, -1)
datos_articulaciones.close()
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
#/piernas/piernas_controller/follow_joint_trajectory
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = piernas_goal
arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[1].positions = piernas_goal12
arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[1].time_from_start = rospy.Duration(4.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[2].positions = piernas_goal2
arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[2].time_from_start = rospy.Duration(6.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[3].positions = piernas_goal3
arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[3].time_from_start = rospy.Duration(8.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[4].positions = piernas_goal121
arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[4].time_from_start = rospy.Duration(10.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[5].positions = piernas_goal25
arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[5].time_from_start = rospy.Duration(12.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[6].positions = piernas_goal341
arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[6].time_from_start = rospy.Duration(14.0)
'''arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[7].positions = piernas_goal12
arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[7].time_from_start = rospy.Duration(17.5)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[8].positions = piernas_goal2
arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[8].time_from_start = rospy.Duration(19.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[9].positions = piernas_goal3
arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[9].time_from_start = rospy.Duration(21.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[10].positions = piernas_goal121
arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[10].time_from_start = rospy.Duration(23.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[11].positions = piernas_goal25
arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[11].time_from_start = rospy.Duration(25.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[12].positions = piernas_goal341
arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)'''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
piernas_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
piernas_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
piernas_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
arm_client.send_goal(piernas_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result()
print arm_client.get_result()
rospy.loginfo('...done')
def __init__(self):
#filename = '/home/kaiser/WS_ROS/catkin_ws/src/abb_experimental-indigo-devel/abb_irb120_gazebo/urdf/modelo_exportado.urdf'
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/IRB120_URDF_v2/robots/IRB120_URDF_v2.URDF'
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("base_link", "link_6")
print chain.getNrOfSegments()
solverCinematicaDirecta = PyKDL.ChainFkSolverPos_recursive(chain)
nj_izq = chain.getNrOfJoints()
posicionInicial = PyKDL.JntArray(nj_izq)
posicionInicial[0] = 0
posicionInicial[1] = 0
posicionInicial[2] = 0
posicionInicial[3] = 0
posicionInicial[4] = 0
posicionInicial[5] = 0
print "Forward kinematics"
finalFrame = PyKDL.Frame()
solverCinematicaDirecta.JntToCart(posicionInicial, finalFrame)
print "Rotational Matrix of the final Frame: "
print finalFrame.M
print "End-effector position: ", finalFrame.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
arm_joints = ['joint_1',
'joint_2',
'joint_3',
'joint_4',
'joint_5',
'joint_6']
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6]
print "Inverse Kinematics"
q_init = posicionInicial # initial angles
vik = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, solverCinematicaDirecta, vik)
#desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame
desiredFrame.p[0] = finalFrame.p[0]
desiredFrame.p[1] = finalFrame.p[1]
desiredFrame.p[2] = finalFrame.p[2]-0.25
print "Desired Position: ", desiredFrame.p
q_out = PyKDL.JntArray(6)
ik.CartToJnt(q_init, desiredFrame, q_out)
print "Output angles in rads: ", q_out
arm_goal[0] = q_out[0]
arm_goal[1] = q_out[1]
arm_goal[2] = q_out[2]
arm_goal[3] = q_out[3]
arm_goal[4] = q_out[4]
arm_goal[5] = q_out[5]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
pub = rospy.Publisher('joint_path_command',JointTrajectory,queue_size=10)
#arm_client = actionlib.SimpleActionClient('joint_trajectory_action', FollowJointTrajectoryAction)
#arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
#arm_client.send_goal(arm_goal)
pub.publish(arm_trajectory)
while not rospy.is_shutdown():
pub.publish(arm_trajectory)
rospy.sleep(rospy.Duration(0.5))
#if not sync:
# Wait for up to 5 seconds for the motion to complete
#arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_cuadrupedo_exp.urdf'
angulo_avance = +0.4 #rad
altura_pata = +0.06 #cm
avance_x = 0.03
# angulo_avance = 0 # +0.40 #rad
# altura_pata = 0 # -0.04 #cm
avance_x = 0.11
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_RR = tree.getChain("base_link", "tarsus_RR")
cadena_RF = tree.getChain("base_link", "tarsus_RF")
cadena_LR = tree.getChain("base_link", "tarsus_LR")
cadena_LF = tree.getChain("base_link", "tarsus_LF")
print cadena_RR.getNrOfSegments()
fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR)
fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF)
fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR)
fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF)
vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR)
ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR)
vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF)
ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF)
vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR)
ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR)
vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF)
ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF)
nj_izq = cadena_RR.getNrOfJoints()
posicionInicial_R = PyKDL.JntArray(nj_izq)
posicionInicial_R[0] = 0
posicionInicial_R[1] = 0
posicionInicial_R[2] = 0
posicionInicial_R[3] = 0
nj_izq = cadena_LR.getNrOfJoints()
posicionInicial_L = PyKDL.JntArray(nj_izq)
posicionInicial_L[0] = 0
posicionInicial_L[1] = 0
posicionInicial_L[2] = 0
posicionInicial_L[3] = 0
print "Forward kinematics"
finalFrame_R = PyKDL.Frame()
finalFrame_L = PyKDL.Frame()
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints_RR = [
'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR',
'tarsus_joint_RR'
]
piernas_joints_RF = [
'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF',
'tarsus_joint_RF'
]
piernas_joints_LR = [
'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR',
'tarsus_joint_LR'
]
piernas_joints_LF = [
'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
'tarsus_joint_LF'
]
rr_goal0 = [0.0, 0.0, 0.0, 0.0]
rf_goal0 = [0.0, 0.0, 0.0, 0.0]
rr_goal1 = [0.0, 0.0, 0.0, 0.0]
rf_goal1 = [0.0, 0.0, 0.0, 0.0]
lr_goal0 = [0.0, 0.0, 0.0, 0.0]
lf_goal0 = [0.0, 0.0, 0.0, 0.0]
lr_goal1 = [0.0, 0.0, 0.0, 0.0]
lf_goal1 = [0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R)
q_init_RR = posicionInicial_R # initial angles
desiredFrameRR = finalFrame_R
desiredFrameRR.p[0] = finalFrame_R.p[0] + avance_x
desiredFrameRR.p[1] = finalFrame_R.p[1]
desiredFrameRR.p[2] = finalFrame_R.p[2] + altura_pata
print "Desired Position: ", desiredFrameRR.p
q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
print "Output angles in rads: ", q_out_RR
rr_goal0[0] = q_out_RR[0]
rr_goal0[1] = q_out_RR[1] #+angulo_avance
rr_goal0[2] = q_out_RR[2]
rr_goal0[3] = q_out_RR[3]
print "Inverse Kinematics"
fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L)
q_init_LF = posicionInicial_L # initial angles
desiredFrameLF = finalFrame_L
desiredFrameLF.p[0] = finalFrame_L.p[0] + avance_x
desiredFrameLF.p[1] = finalFrame_L.p[1]
desiredFrameLF.p[2] = finalFrame_L.p[2] + altura_pata
print "Desired Position: ", desiredFrameLF.p
q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
print "Output angles in rads: ", q_out_LF
lf_goal0[0] = q_out_LF[0]
lf_goal0[1] = q_out_LF[1] #- angulo_avance
lf_goal0[2] = q_out_LF[2]
lf_goal0[3] = q_out_LF[3]
# 22222222222222222222222222222222222222222222
RR_actual = PyKDL.JntArray(nj_izq)
RR_actual[0] = rr_goal0[0]
RR_actual[1] = rr_goal0[1]
RR_actual[2] = rr_goal0[2]
RR_actual[3] = rr_goal0[3]
print "Inverse Kinematics"
fksolver_RR.JntToCart(RR_actual, finalFrame_R)
q_init_RR = RR_actual # initial angles
desiredFrameRR = finalFrame_R
desiredFrameRR.p[0] = finalFrame_R.p[0] - avance_x
desiredFrameRR.p[1] = finalFrame_R.p[1]
desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata
print "Desired Position: ", desiredFrameRR.p
q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
print "Output angles in rads: ", q_out_RR
rr_goal1[0] = q_out_RR[0]
rr_goal1[1] = q_out_RR[1]
rr_goal1[2] = q_out_RR[2]
rr_goal1[3] = q_out_RR[3]
LF_actual = PyKDL.JntArray(nj_izq)
LF_actual[0] = lf_goal0[0]
LF_actual[1] = lf_goal0[1]
LF_actual[2] = lf_goal0[2]
LF_actual[3] = lf_goal0[3]
print "Inverse Kinematics"
fksolver_LF.JntToCart(LF_actual, finalFrame_L)
q_init_LF = LF_actual # initial angles
desiredFrameLF = finalFrame_L
desiredFrameLF.p[0] = finalFrame_L.p[0] - avance_x
desiredFrameLF.p[1] = finalFrame_L.p[1]
desiredFrameLF.p[2] = finalFrame_L.p[2] - altura_pata
print "Desired Position: ", desiredFrameLF.p
q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
print "Output angles in rads: ", q_out_LF
lf_goal1[0] = q_out_LF[0]
lf_goal1[1] = q_out_LF[1] # - angulo_avance
lf_goal1[2] = q_out_LF[2]
lf_goal1[3] = q_out_LF[3]
# 11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L)
q_init_LR = posicionInicial_L # initial angles
desiredFrameLR = finalFrame_L
desiredFrameLR.p[0] = finalFrame_L.p[0] #-avance_x
desiredFrameLR.p[1] = finalFrame_L.p[1]
desiredFrameLR.p[2] = finalFrame_L.p[2] # + altura_pata
print "Desired Position: ", desiredFrameLR.p
q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
print "Output angles in rads: ", q_out_LR
lr_goal0[0] = q_out_LR[0]
lr_goal0[1] = q_out_LR[1] + angulo_avance
lr_goal0[2] = q_out_LR[2]
lr_goal0[3] = q_out_LR[3]
print "Inverse Kinematics"
fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R)
q_init_RF = posicionInicial_R # initial angles
desiredFrameRF = finalFrame_R
desiredFrameRF.p[0] = finalFrame_R.p[0] # -avance_x
desiredFrameRF.p[1] = finalFrame_R.p[1]
desiredFrameRF.p[2] = finalFrame_R.p[2] #+ altura_pata
print "Desired Position: ", desiredFrameRF.p
q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
print "Output angles in rads: ", q_out_RF
rf_goal0[0] = q_out_RF[0]
rf_goal0[1] = q_out_RF[1] - angulo_avance
rf_goal0[2] = q_out_RF[2]
rf_goal0[3] = q_out_RF[3]
# 2222222222222222222222222222222222222222222222
LR_actual = PyKDL.JntArray(nj_izq)
LR_actual[0] = lr_goal0[0]
LR_actual[1] = lr_goal0[1]
LR_actual[2] = lr_goal0[2]
LR_actual[3] = lr_goal0[3]
print "Inverse Kinematics"
fksolver_LR.JntToCart(LR_actual, finalFrame_L)
q_init_LR = LR_actual # initial angles
print "Inverse Kinematics"
desiredFrameLR = finalFrame_L
desiredFrameLR.p[0] = finalFrame_L.p[0] #+ avance_x
desiredFrameLR.p[1] = finalFrame_L.p[1]
desiredFrameLR.p[2] = finalFrame_L.p[2] # - altura_pata
print "Desired Position: ", desiredFrameLR.p
q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
print "Output angles in rads: ", q_out_LR
lr_goal1[0] = q_out_LR[0] #- angulo_avance
lr_goal1[1] = q_out_LR[1]
lr_goal1[2] = q_out_LR[2]
lr_goal1[3] = q_out_LR[3] + angulo_avance
RF_actual = PyKDL.JntArray(nj_izq)
RF_actual[0] = rf_goal0[0]
RF_actual[1] = rf_goal0[1]
RF_actual[2] = rf_goal0[2]
RF_actual[3] = rf_goal0[3]
print "Inverse Kinematics"
fksolver_RF.JntToCart(RF_actual, finalFrame_R)
q_init_RF = RF_actual # initial angles
desiredFrameRF = finalFrame_R
desiredFrameRF.p[0] = finalFrame_R.p[0] # + avance_x
desiredFrameRF.p[1] = finalFrame_R.p[1]
desiredFrameRF.p[2] = finalFrame_R.p[2] #- altura_pata
print "Desired Position: ", desiredFrameRF.p
q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
print "Output angles in rads: ", q_out_RF
rf_goal1[0] = q_out_RF[0]
rf_goal1[1] = q_out_RF[1]
rf_goal1[2] = q_out_RF[2]
rf_goal1[3] = q_out_RF[3] + angulo_avance
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
rr_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_rr/follow_joint_trajectory',
FollowJointTrajectoryAction)
rr_client.wait_for_server()
rf_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_rf/follow_joint_trajectory',
FollowJointTrajectoryAction)
rf_client.wait_for_server()
lr_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_lr/follow_joint_trajectory',
FollowJointTrajectoryAction)
lr_client.wait_for_server()
lf_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_lf/follow_joint_trajectory',
FollowJointTrajectoryAction)
lf_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
rr_trajectory1 = JointTrajectory()
rr_trajectory1.joint_names = piernas_joints_RR
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[0].positions = rr_goal0
rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[1].positions = rr_goal1
rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[2].positions = rf_goal0
rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[3].positions = rf_goal1
rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
#'''
rf_trajectory1 = JointTrajectory()
rf_trajectory1.joint_names = piernas_joints_RF
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[0].positions = rf_goal0 # [0,0,0,0]
rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[1].positions = rf_goal1 # [0,0,0,0]
rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[2].positions = rr_goal0 # [0,0,0,0]
rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[3].positions = rr_goal1 # [0,0,0,0]
rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
lr_trajectory1 = JointTrajectory()
lr_trajectory1.joint_names = piernas_joints_LR
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[0].positions = lr_goal0 #lr_goal0
lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[1].positions = lr_goal1
lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[2].positions = lf_goal0
lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[3].positions = lf_goal1 # lr_goal0
lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
lf_trajectory1 = JointTrajectory()
lf_trajectory1.joint_names = piernas_joints_LF
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[0].positions = lf_goal0 #lf_goal0 # [0,0,0,0]
lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[1].positions = lf_goal1 # [0,0,0,0]
lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[2].positions = lr_goal0 # [0,0,0,0]
lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[3].positions = lr_goal1 # lf_goal0 # [0,0,0,0]
lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
lf_reposo_trajectory1 = JointTrajectory()
lf_reposo_trajectory1.joint_names = piernas_joints_LF
lf_reposo_trajectory1.points.append(JointTrajectoryPoint())
lf_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lr_reposo_trajectory1 = JointTrajectory()
lr_reposo_trajectory1.joint_names = piernas_joints_LR
lr_reposo_trajectory1.points.append(JointTrajectoryPoint())
lr_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rr_reposo_trajectory1 = JointTrajectory()
rr_reposo_trajectory1.joint_names = piernas_joints_RR
rr_reposo_trajectory1.points.append(JointTrajectoryPoint())
rr_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rf_reposo_trajectory1 = JointTrajectory()
rf_reposo_trajectory1.joint_names = piernas_joints_RF
rf_reposo_trajectory1.points.append(JointTrajectoryPoint())
rf_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
# Create an empty trajectory goal
rr_goal = FollowJointTrajectoryGoal()
lm_goal = FollowJointTrajectoryGoal()
rf_goal = FollowJointTrajectoryGoal()
lr_goal = FollowJointTrajectoryGoal()
rm_goal = FollowJointTrajectoryGoal()
lf_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
rr_goal.trajectory = rr_trajectory1
rf_goal.trajectory = rf_trajectory1
lr_goal.trajectory = lr_trajectory1
lf_goal.trajectory = lf_trajectory1
# Specify zero tolerance for the execution time
rr_goal.goal_time_tolerance = rospy.Duration(0.0)
lm_goal.goal_time_tolerance = rospy.Duration(0.0)
rf_goal.goal_time_tolerance = rospy.Duration(0.0)
lr_goal.goal_time_tolerance = rospy.Duration(0.0)
rm_goal.goal_time_tolerance = rospy.Duration(0.0)
lf_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
#rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
#lr_client.wait_for_result(rospy.Duration(5.0))
#rr_client.send_goal(rr_goal)
#lm_client.send_goal(lm_goal)
#rf_client.send_goal(rf_goal)'''
if not sync:
# Wait for up to 5 seconds for the motion to complete
rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
print rr_client.get_result()
rr_goal.trajectory = rr_reposo_trajectory1
rf_goal.trajectory = rf_reposo_trajectory1
lr_goal.trajectory = lr_reposo_trajectory1
lf_goal.trajectory = lf_reposo_trajectory1
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rospy.loginfo('...done')
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 create_solvers(self, ch):
fk = kdl.ChainFkSolverPos_recursive(ch)
ik_v = kdl.ChainIkSolverVel_pinv(ch)
ik_p = kdl.ChainIkSolverPos_NR(ch, fk, ik_v)
jac = kdl.ChainJntToJacSolver(ch)
return fk, ik_v, ik_p, jac
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
robot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf")
tree = kdl_tree_from_urdf_model(robot)
print tree.getNrOfSegments()
chain = tree.getChain("base_link", "sensor_link")
print chain.getNrOfSegments()
print chain.getNrOfJoints()
#正运动学
fk = PyKDL.ChainFkSolverPos_recursive(chain)
pos = PyKDL.Frame()
q = PyKDL.JntArray(7)
for i in range(7):
q[i] = 0
q[0] = 1
fk_flag = fk.JntToCart(q, pos)
print "fk_flag", fk_flag
print "pos", pos
#逆运动学
ik_v = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9))
qq = PyKDL.JntArray(7)
qq_k = PyKDL.JntArray(7)
ik.CartToJnt(qq_k, pos, qq)
print "qq:", qq
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado3.urdf'
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")
print cadena_der_up_down.getNrOfSegments()
fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(
cadena_der_up_down)
fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(
cadena_der_down_up)
fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(
cadena_izq_up_down)
fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(
cadena_izq_down_up)
vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down,
fksolver_der_up_down,
vik_der_up_down)
vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up,
fksolver_der_down_up,
vik_der_down_up)
vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down,
fksolver_izq_up_down,
vik_izq_up_down)
vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up,
fksolver_izq_down_up,
vik_izq_down_up)
nj_izq = cadena_der_up_down.getNrOfJoints()
posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_der_up_down[0] = 0.3
posicionInicial_der_up_down[1] = -0.3
posicionInicial_der_up_down[2] = 0
posicionInicial_der_up_down[3] = 0.6
posicionInicial_der_up_down[4] = -0.3
posicionInicial_der_up_down[5] = -0.3
nj_izq = cadena_izq_up_down.getNrOfJoints()
posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_izq_up_down[0] = 0.3
posicionInicial_izq_up_down[1] = -0.3
posicionInicial_izq_up_down[2] = 0
posicionInicial_izq_up_down[3] = 0.6
posicionInicial_izq_up_down[4] = -0.3
posicionInicial_izq_up_down[5] = -0.3
nj_izq = cadena_der_down_up.getNrOfJoints()
posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_der_down_up[5] = 0.3
posicionInicial_der_down_up[4] = -0.3
posicionInicial_der_down_up[3] = 0
posicionInicial_der_down_up[2] = 0.6
posicionInicial_der_down_up[1] = -0.3
posicionInicial_der_down_up[0] = -0.3
nj_izq = cadena_izq_down_up.getNrOfJoints()
posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_izq_down_up[5] = 0.3
posicionInicial_izq_down_up[4] = -0.3
posicionInicial_izq_down_up[3] = 0
posicionInicial_izq_down_up[2] = 0.6
posicionInicial_izq_down_up[1] = -0.3
posicionInicial_izq_down_up[0] = -0.3
print "Forward kinematics"
finalFrame_izq_up_down = PyKDL.Frame()
finalFrame_izq_down_up = PyKDL.Frame()
finalFrame_der_up_down = PyKDL.Frame()
finalFrame_der_down_up = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down,
finalFrame_izq_up_down)
print "Rotational Matrix of the final Frame: "
print finalFrame_izq_up_down.M
print "End-effector position: ", finalFrame_izq_up_down.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = [
'cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint',
'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint',
'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
]
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
q_init = posicionInicial_izq_up_down # initial angles
#desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame_izq_up_down
desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
print "Desired Position: ", desiredFrame.p
q_out = PyKDL.JntArray(6)
ik_izq_up_down.CartToJnt(q_init, desiredFrame, q_out)
print "Output angles in rads: ", q_out
#arm_goal[0] = q_out[0]
#arm_goal[1] = q_out[1]
#arm_goal[2] = q_out[2]
#arm_goal[3] = q_out[3]
#arm_goal[4] = q_out[4]
#arm_goal[5] = q_out[5]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient(
'/piernas/piernas_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
import time
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import Twist, Point
import kdl_parser_py.urdf
import PyKDL as kdl
import tf
servo = serial.Serial("/dev/ttyAMA0", 115200)
time.sleep(0.5)
filename = "../urdf/simple_arm.urdf"
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("base", "link7")
t = kdl.Tree(tree)
fksolverpos = kdl.ChainFkSolverPos_recursive(chain)
iksolvervel = kdl.ChainIkSolverVel_pinv(chain)
iksolverpos = kdl.ChainIkSolverPos_NR(chain, fksolverpos, iksolvervel)
class xero(object):
def __init__(self):
self._sub = rospy.Subscriber("cntl", Twist, self._callback)
self.msg = Float32MultiArray()
self.cmd = Twist()
self.last_cmd = Twist()
self.R = Point()
self.L = Point()
def _callback(self, cmd):
# print (cmd.angular.z, self.last_cmd.angular.z)
if cmd.angular.z != self.last_cmd.angular.z:
