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, 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 __inverse_kinematics(self, position, orientation=None, seed=None):
"""inverse kinematic solver using PyKDL"""
_inverse_kin_position_kdl = PyKDL.ChainIkSolverPos_NR_JL(self.iiwa_chain, self.min_limits, self.max_limits,
self.forward_kin_position_kdl, self.inverse_kin_velocity_kdl)
ik = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain)
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation != None:
rot = PyKDL.Rotation()
#PyKDL uses w, x, y, z instead of x, y, z, w
rot = rot.Quaternion(orientation[1], orientation[2], orientation[3], orientation[0])
seed_array = PyKDL.JntArray(NUM_JOINTS)
if seed != None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
joint_vals = self.group.get_current_joint_values()
for idx, jnt in enumerate(joint_vals):
seed_array[idx] = joint_vals[idx]
if orientation:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(NUM_JOINTS)
if _inverse_kin_position_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(result_angles)
return result
else:
print 'No IK Solution Found'
return None
def inverse_kinematics(self, position, orientation=None, seed=None):
ik = PyKDL.ChainIkSolverVel_pinv(self._kdl_chain)
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation is not None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed is not None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self._joints_to_kdl()
# Make IK Call
if orientation is not None:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
num_tries = 0
status = self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles)
if status < 0:
print "IK solution not found with current joint angles as seed. Trying random seeds..."
while status < 0 and num_tries < 100:
seed_array = self._get_random_seed_array()
status = self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles)
num_tries += 1
if status < 0:
print "No IK solution found."
return None
else:
return np.array(result_angles)
def __init__(self,
robot,
base_link=None,
tip_link=None,
data_path="../.."):
cur_path = os.path.dirname(os.path.abspath(__file__))
if PYTHON2:
import cPickle as pickle
with open(cur_path + "/robot.pkl", "rb") as fid:
robot_info = pickle.load(fid)
else:
import pickle
with open(cur_path + "/robot_p3.pkl", "rb") as fid:
robot_info = pickle.load(fid)
self._pose_0 = robot_info["_pose_0"]
self.finger_pose = self._pose_0[-2].copy()
self._joint_origin = robot_info["_joint_axis"]
self._tip2joint = robot_info["_tip2joint"]
self._joint_axis = robot_info["_joint_axis"]
self._joint_limits = robot_info["_joint_limits"]
self._joint2tips = robot_info["_joint2tips"]
self._joint_name = robot_info["_joint_name"]
self.center_offset = np.array(robot_info["center_offset"])
self._link_names = robot_info["_link_names"]
self._base_link, self._tip_link = "panda_link0", "panda_hand"
self._num_jnts = 7
self._robot = URDF.from_xml_string(
open(
os.path.join(cur_path, data_path, "data/robots",
"panda_arm_hand.urdf"),
"r",
).read())
self._kdl_tree, _ = kdl_tree_from_urdf_model(self._robot)
self.soft_joint_limit_padding = 0.2
mins_kdl = joint_list_to_kdl(
np.array([
self._joint_limits[n][0] + self.soft_joint_limit_padding
for n in self._joint_name[:-3]
]))
maxs_kdl = joint_list_to_kdl(
np.array([
self._joint_limits[n][1] - self.soft_joint_limit_padding
for n in self._joint_name[:-3]
]))
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
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_JL(self._arm_chain,
mins_kdl, maxs_kdl,
self._fk_p_kdl,
self._ik_v_kdl)
def inverse_kinematics(self, position, orientation=None, seed=None):
ik = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation != None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed != None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self.joints_to_kdl('positions')
# Make IK Call
if orientation:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
return result
else:
return None
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 get_kinematics_solvers(kdl_chain, kdl_limits_min, kdl_limits_max):
fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain)
velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain)
# ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15)
ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_limits_min,
kdl_limits_max, fk_solver,
velocity_ik)
return fk_solver, ik_solver
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 calculate_ik(base_link, tip_link, seed_joint_state,
goal_transform_geometry_msg):
"""
Calculates the Inverse Kinematics from base_link to tip_link according to the given
goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state.
Returns the result joint states and the success status.
base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link"
tip_link eg. - "left_arm_7_link" or "right_arm_7_link"
"""
robot_urdf_string = rospy.get_param('robot_description')
urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string)
_, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj)
kdl_chain = kdl_tree.getChain(base_link, tip_link)
num_joints = kdl_chain.getNrOfJoints()
print "number of joints: " + str(num_joints)
# Get Joint limits
kdl_joint_limits_min, kdl_joint_limits_max = get_kdl_joint_limit_arrays(
kdl_chain, urdf_obj)
fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain)
velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain)
# ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15)
ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_joint_limits_min,
kdl_joint_limits_max, fk_solver,
velocity_ik)
# Getting the goal frame and seed state
goal_frame_kdl = tf2_kdl.transform_to_kdl(goal_transform_geometry_msg)
seed_joint_state_kdl = get_kdl_jnt_array_from_list(num_joints,
seed_joint_state)
# Solving IK
result_joint_state_kdl = solve_ik(ik_solver, num_joints,
seed_joint_state_kdl, goal_frame_kdl)
# check if calculated joint state results in the correct end-effector position using FK
goal_pose_reached = check_ik_result_using_fk(fk_solver,
result_joint_state_kdl,
goal_frame_kdl)
# check if calculated joint state is within joint limits
joints_within_limits = check_result_joints_are_within_limits(
num_joints, result_joint_state_kdl, kdl_joint_limits_min,
kdl_joint_limits_max)
print "Result Joint State Within Limits: " + str(joints_within_limits)
print "Can Reach Goal Pose With Solution: " + str(goal_pose_reached)
result_joint_state_vector = get_list_from_kdl_jnt_array(
num_joints, result_joint_state_kdl)
goal_pose_reached_successfully = goal_pose_reached and joints_within_limits
return result_joint_state_vector, goal_pose_reached_successfully
def __init__(self):
self.base_link = 'base_link'
self.ee_link = 'ee_link'
flag, self.tree = kdl_parser.treeFromParam('/robot_description')
self.chain = self.tree.getChain(self.base_link, self.ee_link)
self.num_joints = self.tree.getNrOfJoints()
self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)
self.cam_model = image_geometry.PinholeCameraModel()
self.joint_state = kdl.JntArrayVel(self.num_joints)
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rospy.init_node('ur_eef_vel_controller')
rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
rospy.Subscriber('/rdda_interface/joint_states', JointState,
self.finger_joint_state_cb)
self.joint_vel_cmd_pub = rospy.Publisher(
'/joint_group_vel_controller/command',
Float64MultiArray,
queue_size=10)
self.joint_pos_cmd_pub = rospy.Publisher(
'/scaled_pos_traj_controller/command',
JointTrajectory,
queue_size=10)
self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
Float64,
queue_size=10)
self.switch_controller_cli = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.joint_pos_cli = actionlib.SimpleActionClient(
'/scaled_pos_joint_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
cam_info = rospy.wait_for_message('/camera/depth/camera_info',
CameraInfo,
timeout=10)
self.cam_model.fromCameraInfo(cam_info)
self.bridge = CvBridge() # allows conversion from depth image to array
self.tf_listener = tf.TransformListener()
self.image_offset = 100
self.pos_ctrler_running = False
rospy.sleep(0.5)
def _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 inverse_kinematics(self,
position,
orientation=None,
seed=None,
min_joints=None,
max_joints=None,
maxiter=500,
eps=1.0e-6):
ik = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation is not None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed is not None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self.joints_to_kdl('positions')
# Make IK Call
if orientation is not None:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
# Make IK solver with joint limits
if min_joints is None: min_joints = self.joint_limits_lower
if max_joints is None: max_joints = self.joint_limits_upper
mins_kdl = PyKDL.JntArray(len(min_joints))
for idx, jnt in enumerate(min_joints):
mins_kdl[idx] = jnt
maxs_kdl = PyKDL.JntArray(len(max_joints))
for idx, jnt in enumerate(max_joints):
maxs_kdl[idx] = jnt
ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
self._ik_v_kdl, maxiter, eps)
if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
return result
else:
return None
def inverse_kinematics(robot_chain,
pos,
rot,
q_guess=None,
min_joints=None,
max_joints=None):
"""
Perform inverse kinematics
Args:
robot_chain: robot's chain object
pos: 1 x 3 or 3 x 1 array of the end effector position.
rot: 3 x 3 array of the end effector rotation
q_guess: guess values for the joint positions
min_joints: minimum value of the position for each joint
max_joints: maximum value of the position for each joint
Returns:
list of joint positions or None (no solution)
"""
pos_kdl = kdl.Vector(pos[0], pos[1], pos[2])
rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0],
rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2,
2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
num_joints = robot_chain.getNrOfJoints()
min_joints = -np.pi * np.ones(
num_joints) if min_joints is None else min_joints
max_joints = np.pi * np.ones(
num_joints) if max_joints is None else max_joints
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
fk_kdl = kdl.ChainFkSolverPos_recursive(robot_chain)
ik_v_kdl = kdl.ChainIkSolverVel_pinv(robot_chain)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(robot_chain, mins_kdl, maxs_kdl,
fk_kdl, ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess)
q_kdl = kdl.JntArray(num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return joint_kdl_to_list(q_kdl)
else:
return None
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 initialize_kinematic_solvers(self):
robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
self._robot = URDF.from_parameter_server(key=robot_description)
self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
# self._base_link = self._robot.get_root()
self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
self._ee_frame = PyKDL.Frame()
self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
class MoveGroupLeftArm(object):
def __init__(self):
#----------------control init begin-----------------------------------------------
super(MoveGroupLeftArm, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('iiwa_move_to_ee_pose', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
self.group = moveit_commander.MoveGroupCommander(group_name)
#group.set_max_velocity_scaling_factor(0.15)
self.group.set_max_acceleration_scaling_factor(0.1)
if self.group.set_planner_id(PLANNER_ID):
print "Using planner: %s" % PLANNER_ID
self.group.set_num_planning_attempts(100)
self.group.set_planning_time(3)
self.group.set_start_state_to_current_state()
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
self.gripper_io_publisher = rospy.Publisher('command/GripperState', iiwa_msgs.msg.GripperState, queue_size=10)
self.upright_constraints = Constraints()
try:
rospy.wait_for_service('configuration/setSmartServoLimits', 1)
except:
print 'Service call timeout'
try:
self.set_speed_limits = rospy.ServiceProxy('configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits)
response = self.set_speed_limits(0.3, 0.1, -1)
print 'Velocity limit set'
print response
except rospy.ServiceException, e:
print "service call failed: %s"%e
#----------------kinematics init begin---------------------------------------------
_iiwa_URDF = URDF.from_parameter_server(key='robot_description')
_iiwa_kdl_tree = kdl_tree_from_urdf_model(_iiwa_URDF)
_iiwa_base_link = _iiwa_URDF.get_root()
self.iiwa_chain = _iiwa_kdl_tree.getChain(_iiwa_base_link, 'tool_link_ee')
self.forward_kin_position_kdl = PyKDL.ChainFkSolverPos_recursive(self.iiwa_chain)
_forward_kin_velocity_kdl = PyKDL.ChainFkSolverVel_recursive(self.iiwa_chain)
self.inverse_kin_velocity_kdl = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain)
self.min_limits = PyKDL.JntArray(NUM_JOINTS)
self.max_limits = PyKDL.JntArray(NUM_JOINTS)
for idx, jnt in enumerate(MIN_JOINT_LIMITS_DEG):
self.min_limits[idx] = math.radians(jnt)
for idx, jnt in enumerate(MAX_JOINT_LIMITS_DEG):
self.max_limits[idx] = math.radians(jnt)
self.component_map = {}
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 inverseKinematics(robotChain, pos, rot, qGuess=None, minJoints=None, maxJoints=None):
"""
Perform inverse kinematics
Args:
robotChain: robot's chain object
pos: 1 x 3 or 3 x 1 array of the end effector position.
rot: 3 x 3 array of the end effector rotation
qGuess: guess values for the joint positions
minJoints: minimum value of the position for each joint
maxJoints: maximum value of the position for each joint
Returns:
list of joint positions or None (no solution)
"""
# print("inside inverse: ", pos, " ; ", rot)
posKdl = kdl.Vector(pos[0], pos[1], pos[2])
rotKdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2],
rot[1, 0], rot[1, 1], rot[1, 2],
rot[2, 0], rot[2, 1], rot[2, 2])
frameKdl = kdl.Frame(rotKdl, posKdl)
numJoints = robotChain.getNrOfJoints()
minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints
maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints
minsKdl = jointListToKdl(minJoints)
maxsKdl = jointListToKdl(maxJoints)
fkKdl = kdl.ChainFkSolverPos_recursive(robotChain)
ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain)
ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl,
fkKdl, ikVKdl)
if qGuess is None:
# use the midpoint of the joint limits as the guess
lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.)
upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.)
qGuess = (lowerLim + upperLim) / 2.0
qGuess = np.where(np.isnan(qGuess), [0.]*len(qGuess), qGuess)
qKdl = kdl.JntArray(numJoints)
qGuessKdl = jointListToKdl(qGuess)
if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0:
return jointKdlToList(qKdl)
else:
return None
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 __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()
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.Zero())
def __init__(self, T=20, weight=[1.0,1.0]):
#initialize model
self.chain = PyKDL.Chain()
self.chain.addSegment(PyKDL.Segment("Base", PyKDL.Joint(PyKDL.Joint.None), PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)), PyKDL.RigidBodyInertia(0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint1", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)), PyKDL.RigidBodyInertia(0.00795, PyKDL.Vector(0.0, 0.019, -0.02025), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint2", PyKDL.Joint(PyKDL.Joint.RotY), PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint3", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)), PyKDL.RigidBodyInertia(0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint4", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint5", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint6", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(PyKDL.Segment("Joint7", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)), PyKDL.RigidBodyInertia(0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.min_position_limit = [-160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0]
self.max_position_limit = [160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0]
self.min_joint_position_limit = PyKDL.JntArray(7)
self.max_joint_position_limit = PyKDL.JntArray(7)
for i in range (0,7):
self.min_joint_position_limit[i] = self.min_position_limit[i]*pi/180
self.max_joint_position_limit[i] = self.max_position_limit[i]*pi/180
self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain)
self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.min_joint_position_limit, self.max_joint_position_limit, self.fksolver, self.iksolver, 100, 1e-6)
#parameter
self.T = T
self.weight_u = weight[0]
self.weight_x = weight[1]
self.nj = self.chain.getNrOfJoints()
self.q_init = np.zeros(self.nj)
self.q_out = np.zeros(self.nj)
ret, self.dest, self.q_out = self.generate_dest()
self.fin_position = self.dest.p
return
def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Create KDL model
with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings
self.robot = URDF.from_parameter_server() # sawyer's URDF
# self.kin = KDLKinematics(self.robot, "base", "right_gripper") # kinematics to custom joint frame
self.kin = KDLKinematics(self.robot, "base", "right_gripper_tip")
self.names = self.kin.get_joint_names()
if rospy.has_param('vel_calc'):
rospy.delete_param('vel_calc')
self.limb = intera_interface.Limb("right")
#self._limb = Limb()
self.gripper = intera_interface.gripper.Gripper('right')
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
self.Kp = 50.0 * np.eye(6)
self.Ki = 0.0 * np.eye(6)
self.Kd = 100.0 * np.eye(6) # add D-gain
self.it_count = 0
self.int_err = 0
self.der_err = 0
self.int_anti_windup = 10
self.rate = 100.0 #Hz
self.sample_time = 1.0 / self.rate * 2.5 #ms
self.USE_FIXED_RATE = False # If true -> dT = 0.01, Else -> function call time difference
self.current_time = time.time()
self.last_time = self.current_time
self.last_error = [0.0, 0.0, 0.0, 0.0, 0.0,
0.0] # error in cartesian space
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit",
JOINT_VEL_LIMIT)
self.q = np.zeros(7) # Joint angles
self.qdot = np.zeros(7) # Joint velocities
self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3
self.cur_frame = pm.fromMatrix(self.T_goal) # -> PyKDL frame
# robot @ its original pose : position (0.3161, 0.1597, 1.151) , orientation (0.337, 0.621, 0.338, 0.621)
self.original_goal = self.T_goal.copy() # robot @ its home pose :
print('Verifying initial pose...')
print(self.T_goal)
self.isCalcOK = False
self.isPathPlanned = False
self.traj_err_bound = float(1e-2) # in meter
self.plan_time = 2.5 # in sec. can this be random variable?
self.rand_plan_min = 5.0
self.rand_plan_max = 9.0
# Subscriber
self.ee_position = [0.0, 0.0, 0.0]
self.ee_orientation = [0.0, 0.0, 0.0, 0.0]
self.ee_lin_twist = [0.0, 0.0, 0.0]
self.ee_ang_twist = [0.0, 0.0, 0.0]
rospy.Subscriber('/demo/target/', Pose, self.ref_poseCB)
self.gripper.calibrate()
# path planning
self.num_wp = int(0)
self.cur_wp_idx = int(0) # [0:num_wp - 1]
self.traj_list = [None for _ in range(self.num_wp)]
self.traj_elapse = 0.0 # in ms
self.r = rospy.Rate(self.rate)
# robot chain for the ik solver of PyKDL
# This constructor parses the URDF loaded in rosparm urdf_param into the
# needed KDL structures.
self._base = self.robot.get_root()
self._tip = "right_gripper_tip"
self.sawyer_tree = kdl_tree_from_urdf_model(
self.robot) # sawyer's kinematic tree
self.sawyer_chain = self.sawyer_tree.getChain(self._base, self._tip)
# KDL solvers
self.ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self.sawyer_chain)
self._num_jnts = 7
self._joint_names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
rospy.loginfo('Check the URDF of sawyer')
self.print_robot_description()
rospy.loginfo('Check the sanity of kinematic chain')
self.print_kdl_chain()
self.prev_frame = PyKDL.Frame() # initialize as identity frame
self.init_frame = PyKDL.Frame(
) # frame of the start pose of the trajectory
self._frame1 = PyKDL.Frame()
self._frame2 = PyKDL.Frame()
self.pose1 = Pose()
self.pose2 = Pose()
rospy.Subscriber("test", itzy, callback=self.msgggg)
self.vr0_T_saw0 = np.identity(4)
self.btn_state = 0
self.btn_state_2 = 0
self._gripper = intera_interface.Gripper()
rospy.Subscriber('vive_right', Joy, callback=self.grip_open_close)
# control loop
rospy.Subscriber('/robot/limb/right/endpoint_state', EndpointState,
self.endpoint_poseCB)
while not rospy.is_shutdown():
# if rospy.has_param('vel_calc'):
# if not self.isPathPlanned: # if path is not planned
# self.path_planning() # get list of planned waypoints
# # self.calc_joint_vel_2()
self.calc_joint_vel_3()
self.r.sleep()
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/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
