def main():
global save, psm1_robot, psm1_kin, psm2_robot, psm2_kin, ecm_robot, ecm_kin
rospy.init_node('psm_optimization_data_collector')
# Get the joint angles from the hardware and move the simulation from hardware
rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, psm1_read_cb)
rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, psm2_read_cb)
rospy.Subscriber('/dvrk/ECM/state_joint_current', JointState, ecm_read_cb)
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[-1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[-1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
while True:
print("save now? ")
print("(y) yes\n(n) no\n(q) quit")
r = raw_input(" : ")
if r == "q":
global file
file.close()
return
if r == "y":
psm1_read_cb.save = True
psm2_read_cb.save = True
ecm_read_cb.save = True
rospy.spin()
def __init__(self, config):
base_link = config['base_link']
end_link = config['end_link']
if 'robot_description_param' in config:
self.robot = URDF.from_parameter_server(
config['robot_description_param'])
else:
self.robot = URDF.from_parameter_server()
self.config = config
self.base_link = base_link
# set up kinematics stuff
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.base_link = base_link
self.end_link = end_link
self.skill_instances = {}
self.skill_features = {}
self.skill_models = {}
self.parent_skills = {}
self.pubs = {}
def __init__(self):
self.t = time.time()
self.__AUTOCAMERA_MODE__ = self.MODE.simulation
self.autocamera = Autocamera() # Instantiate the Autocamera Class
self.jnt_msg = JointState()
self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None}
self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()}
self.last_ecm_jnt_pos = None
self.first_run = True
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.initialize_psms_initialized = 30
self.__DEBUG_GRAPHICS__ = False
def main():
global psm1_kin,psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
objective_function.mode = MODE
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[1].name, psm1_robot.links[-1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[1].name, psm2_robot.links[-1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[3].name, ecm_robot.links[-1].name)
initial_guess = [ (.80,0.5,.3), (0.2,0.7,1.57)]
res = minimize(objective_function, initial_guess, method='nelder-mead', options={'xtol':1e-12, 'disp':True, 'maxiter': 100000, 'maxfev':100000},)
print(res)
print(res.x)
file.close()
print(objective_function.mode)
if objective_function.mode == 'psm2_psm1':
print('psm2 relative to world: ')
v = find_everything_related_to_world('psm2', res.x)
# print("""xyz="{} {} {}" rpy="{} {} {}" """.format(v[0], v[1]) )
print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))
if objective_function.mode == 'ecm_psm1':
print('ecm relative to world: ')
v = find_everything_related_to_world('ecm', res.x)
print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))
def is_shou_yaw_goal_in_range(joint_goal):
# If shoulder yaw goal angle is out of joint range, abort
upper = URDF.from_parameter_server().joint_map["j_shou_yaw"].limit.upper
lower = URDF.from_parameter_server().joint_map["j_shou_yaw"].limit.lower
if (joint_goal[constants.J_SHOU_YAW]<lower) or (joint_goal[constants.J_SHOU_YAW]>upper):
return False
else:
return True
def __init__(self):
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name)
self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None}
self.logerror("autocamera_initialized")
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):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
# Publishes Cartesian goals
self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform,
queue_size=1)
# Publishes Redundancy goals
self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1)
# Publisher to set robot position
self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
#Create "Interactive Marker" that we can manipulate in RViz
self.init_marker()
self.ee_tracking = 0
self.red_tracking = 0
self.q_current = []
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.mutex = Lock()
self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback)
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.joint_callback)
def create_kdl_kin(base_link, end_link, urdf_filename=None):
if urdf_filename is None:
robot = URDF.from_parameter_server()
else:
f = open(urdf_filename, "r")
robot = URDF.from_xml_string(f.read())
return KDLKinematics(robot, base_link, end_link)
def __init__(self):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
self.joint_transforms = []
self.q_current = []
self.q0_desired = 0
self.x_current = tf.transformations.identity_matrix()
self.x_current2 = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix() #???
self.x_desired = tf.transformations.identity_matrix()
# This is where we hold general info about the joints
self.num_joints = 0
self.joint_names = []
self.joint_axes = []
# Prepare general information about the robot
self.get_joint_info()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/cartesian_command", CartesianCommand,
self.command_callback)
rospy.Subscriber("/ik_command", Transform, self.ik_callback)
self.pub_jv = rospy.Publisher("/joint_velocities",
JointState,
queue_size=1)
self.ik = rospy.Publisher("/joint_command", JointState, queue_size=1)
def __init__(self):
#Load robot from parameter server
self.robot = URDF.from_parameter_server()
#Subscribe to current joint state of the robot
rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
#This will load information about the joints of the robot
self.num_joints = 0
self.joint_names = []
self.q_current = []
self.joint_axes = []
self.get_joint_info()
#This is a mutex
self.mutex = Lock()
#Subscribers and publishers for for cartesian control
rospy.Subscriber('/cartesian_command', CartesianCommand, self.get_cartesian_command)
self.velocity_pub = rospy.Publisher('/joint_velocities', JointState, queue_size=10)
self.joint_velocity_msg = JointState()
#Subscribers and publishers for numerical IK
rospy.Subscriber('/ik_command', Transform, self.get_ik_command)
self.joint_command_pub = rospy.Publisher('/joint_command', JointState, queue_size=10)
self.joint_command_msg = JointState()
def __init__(self):
super(TomDataset, self).__init__("TOM")
# hold bag file names
self.trash_bags = []
self.box_bags = []
# hold demos
self.box = []
self.trash = []
self.trash_poses = []
self.box_poses = []
self.move_poses = []
self.lift_poses = []
self.test_poses = []
self.pickup_poses = []
self.trash_data = []
self.box_data = []
self.move_data = []
self.lift_data = []
self.test_data = []
self.pickup_data = []
self.robot = URDF.from_parameter_server()
self.base_link = "torso_link"
self.end_link = "r_gripper_base_link"
# set up kinematics stuff
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(self.base_link, self.end_link)
self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
def __init__(self, side):
# Redirect annoying output of upcoming URDF command
devnull = open(os.devnull, 'w')
sys.stdout, sys.stderr = devnull, devnull
self.robot = URDF.from_parameter_server()
# Now point output back
sys.stdout = sys.__stdout__
sys.stderr = sys.__stderr__
devnull.close()
self.joint_list = {}
for ndx, jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
self.chain = list()
# Query parameter server for joints
arm_chain = '/' + side + '_arm_chain'
joint_names = rospy.get_param(arm_chain)
for joint in joint_names:
self.chain.append(JointData(self, joint))
def __init__(self):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
#Subscribes to command for end-effector pose
rospy.Subscriber("/cartesian_command", Transform,
self.command_callback)
#Subscribes to command for redundant dof
rospy.Subscriber("/redundancy_command", Float32,
self.redundancy_callback)
# Publishes desired joint velocities
self.pub_vel = rospy.Publisher("/joint_velocities",
JointState,
queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.q_current = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.last_command_time = 0
self.last_red_command_time = 0
# Initialize timer that will trigger callbacks
self.mutex = Lock()
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
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):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient(
'/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names = [
'j1', 'j2', 'j3', 'j4', 'j5', 'j6'
]
#To be reached 1 second after starting along the trajectory
trajectory_point = trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions = [0] * 6
trajectory_point.time_from_start = rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState,
self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose,
self.inv_kinematics_cb)
def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None):
self._kdl_tree = 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:
# add additional segments to account for NE_T_EE and F_T_NE transformations
# ---- this may cause issues in eg. inertia computations maybe? TODO: test inertia behaviour
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_frame = PyKDL.Frame()
self._limb_interface = limb
self.create_chain(ee_frame_name)
def __init__(self):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
# This is where we hold general info about the joints
self.joint_names = []
self.joint_axes = []
# Prepare general information about the robot
self.get_joint_info()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
#Subscribes to command for end-effector pose
rospy.Subscriber("/cartesian_command", CartesianCommand,
self.command_callback)
#Subscribes to command for end-effector pose using numerical IK
rospy.Subscriber("/ik_command", Transform, self.IK_command_callback)
# Publishes desired joint velocities
self.pub_vel = rospy.Publisher("/joint_velocities",
JointState,
queue_size=1)
# Publishes direct command for positions
self.pub_command = rospy.Publisher("/joint_command",
JointState,
queue_size=1)
#This is where we hold the most recent joint info
self.current_joint_state = JointState()
def __init__(self):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
# Publishes Cartesian goals
self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform,
queue_size=1)
# Publishes Redundancy goals0.6
#self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1)
# Publisher to set robot position
self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
#Create "Interactive Marker" that we can manipulate in RViz
self.init_marker()
self.ee_tracking = 0
self.red_tracking = 0
self.q_current = []
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.mutex = Lock()
self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback)
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.joint_callback)
def __init__(self):
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base_link')
self.tip_link = read_parameter('~tip_link', 'end_effector')
# Kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
# Get the joint names and limits
self.joint_names = self.kinematics.get_joint_names()
self.num_joints = len(self.joint_names)
# Set-up publishers/subscribers
self.torque_pub = dict()
for i, name in enumerate(self.joint_names):
self.torque_pub[name] = rospy.Publisher('/%s/command' % (name),
Float64)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
rospy.Subscriber('/grips/endpoint_state', EndpointState,
self.endpoint_state_cb)
rospy.Subscriber('/grips/ik_command', PoseStamped, self.ik_command_cb)
rospy.loginfo('Running Cartesian controller for Grips')
# Start torque controller timer
self.cart_command = None
self.endpoint_state = None
self.joint_states = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
def __init__(self, joint_names_vector, inactive_joint_names, js_pos):
self.robot = URDF.from_parameter_server()
self.tree, self.segment_map, self.segment_parent_map, self.segment_name_id_map = self.kdl_tree_from_urdf_model(self.robot, inactive_joint_names, js_pos)
self.segment_id_name_map = {}
for seg_name in self.segment_name_id_map:
seg_id = self.segment_name_id_map[seg_name]
self.segment_id_name_map[seg_id] = seg_name
self.fk_solvers = {}
self.createSegmentToJointMap(joint_names_vector, inactive_joint_names)
joint_limit_map = {}
for j in self.robot.joints:
if j.limit != None:
joint_limit_map[j.name] = j.limit
self.lim_lower = np.empty(len(joint_names_vector))
self.lim_lower_soft = np.empty(len(joint_names_vector))
self.lim_upper = np.empty(len(joint_names_vector))
self.lim_upper_soft = np.empty(len(joint_names_vector))
q_idx = 0
for joint_name in joint_names_vector:
self.lim_lower[q_idx] = joint_limit_map[joint_name].lower
self.lim_lower_soft[q_idx] = self.lim_lower[q_idx] + 15.0/180.0*math.pi
self.lim_upper[q_idx] = joint_limit_map[joint_name].upper
self.lim_upper_soft[q_idx] = self.lim_upper[q_idx] - 15.0/180.0*math.pi
q_idx += 1
def __init__(self, name):
self.elevated_distance = 0.3 # .07
# Find the baxter from the parameter server
self.baxter = URDF.from_parameter_server()
# Note: A node that initializes and runs the baxter has to be running in the background for
# from_parameter_server to to find the parameter.
# Older versions of URDF label the function "load_from_parameter_server"
# Subscribe to the "baxter1_joint_states" topic, which provides the joint states measured by the baxter in a
# ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the
# callback function self.get_joint_states, which is defined below.
self.joint_state_sub = rospy.Subscriber("/robot/joint_states", JointState, self.get_joint_states)
self.end_eff_state_sub = rospy.Subscriber(
"/robot/limb/left/endpoint_state", EndpointState, self.get_end_eff_state
)
self.listener = tf.TransformListener()
# self.timer1 = rospy.Timer(rospy.Duration(0.01),)
# calibrate the gripper
self.initialize_gripper()
self._action_name = name
# The main function of BaxterWeigh is called whenever a client sends a goal to weigh_server
self._as = actionlib.SimpleActionServer(
self._action_name, baxter_pour.msg.WeighAction, execute_cb=self.main, auto_start=False
)
self._as.start()
# To test the node separatly call self.main() manually
# self.main()
return
def __init__(self):
self.robot = URDF.from_parameter_server()
# This is where we hold general info about the joints
self.num_joints = 0
self.link = []
self.joint = []
self.joint_names = []
self.joint_axes = []
# Prepare general information about the robot
self.get_joint_info()
self.current_joint_state = JointState()
# Wait for validity check service
rospy.wait_for_service("check_state_validity")
self.state_valid_service = rospy.ServiceProxy(
'check_state_validity', moveit_msgs.srv.GetStateValidity)
print "State validity service ready"
# MoveIt parameter
if self.num_joints == 7:
self.group_name = "lwr_arm"
elif self.num_joints == 6:
self.group_name = "manipulator"
self.pub_trajectory = rospy.Publisher('/joint_trajectory',
JointTrajectory,
queue_size=10)
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/motion_planning_goal", geometry_msgs.msg.Transform,
self.motion_planning_callback)
def __init__(self):
# Wait for moveit IK service
rospy.wait_for_service("compute_ik")
self.ik_service = rospy.ServiceProxy('compute_ik', moveit_msgs.srv.GetPositionIK)
print "IK service ready"
# Wait for validity check service
rospy.wait_for_service("check_state_validity")
self.state_valid_service = rospy.ServiceProxy('check_state_validity',
moveit_msgs.srv.GetStateValidity)
print "State validity service ready"
# MoveIt parameter
self.group_name = "manipulator"
# Obtain basic information to implement my IK
self.robot = URDF.from_parameter_server()
self.num_joints = 0
self.joints = []
self.joint_names = []
self.all_axes = []
self.current_joint_state = sensor_msgs.msg.JointState()
self.get_joint_info()
# Initialize the publisher
self.pub_trajectory = rospy.Publisher("/joint_trajectory", trajectory_msgs.msg.JointTrajectory, queue_size=1)
# Subscribe to topics
rospy.Subscriber("/joint_states", sensor_msgs.msg.JointState, self.callback_jointstate)
rospy.Subscriber("/motion_planning_goal", geometry_msgs.msg.Transform, self.callback_get_goal)
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):
#Load robot from parameter server
self.robot = URDF.from_parameter_server()
#Subscribe to current joint state of the robot
rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
#This will load information about the joints of the robot
self.num_joints = 0
self.joint_names = []
self.q_current = []
self.joint_axes = []
self.get_joint_info()
#This is a mutex; a mutex(mutual exclusion object)is a program object that is
#created so that multiple program thread can take turns sharing the same resource
self.mutex = Lock()
#Subscribers and publishers for for cartesian control
rospy.Subscriber('/cartesian_command', CartesianCommand,
self.get_cartesian_command)
self.velocity_pub = rospy.Publisher('/joint_velocities',
JointState,
queue_size=10)
self.joint_velocity_msg = JointState()
#Subscribers and publishers for numerical IK
rospy.Subscriber('/ik_command', Transform, self.get_ik_command)
self.joint_command_pub = rospy.Publisher('/joint_command',
JointState,
queue_size=10)
self.joint_command_msg = JointState()
def __init__(self):
rospy.init_node('expert_opt')
# params
self.base_link = rospy.get_param("~base_link", "base_link")
self.biceps_link = rospy.get_param("~biceps_link", "biceps_link")
self.camera_link = rospy.get_param("~camera_link", "camera_color_optical_frame")
self.model_file = rospy.get_param("~model") # required path to model file
self.normp_file = rospy.get_param("~norm_params", "") # optional path to normalization parameters (empty str means no norm params)
# TODO - complete the line below to load up your model and create any necessary class instance variables
self.model = ...
# joint values
self.joint1 = None
self.joint3 = None
# get robot model
self.robot = URDF.from_parameter_server()
# joint publishers
self.joint1_pub = rospy.Publisher('/joint_1/command', Float64, queue_size=5) # command for base rotation
self.joint3_pub = rospy.Publisher('/joint_3/command', Float64, queue_size=5) # command for robot tilt
# joint subscriber
rospy.Subscriber('/joint_states', JointState, self.joints_callback, queue_size=5)
rospy.Subscriber('/target', PoseStamped, self.target_callback, queue_size=5)
rospy.spin()
def main(argv):
root_name,tip_name=readArguments(argv)
robot = URDF.from_parameter_server()
desired_joint_chain=robot.get_chain(root=root_name,tip=tip_name,joints=True,links=False)
desired_link_chain=robot.get_chain(root=root_name,tip=tip_name,joints=False,links=True)
print desired_joint_chain
print desired_link_chain
q=sympy.symbols(desired_joint_chain)
axes,transforms_dh,sigma=getKinematicInformation(robot,root_name,tip_name)
transforms_sym=directGeometricModel(transforms_dh,q,axes,sigma)
# Need deepcopy as transforms_sym is modified by function
jacobians_sym=allDirectKinematicModels(deepcopy(transforms_sym),deepcopy(axes),deepcopy(sigma))
write_transforms(desired_link_chain,desired_joint_chain,transforms_sym)# write transforms to file
write_jacobians(desired_link_chain,desired_joint_chain,jacobians_sym)# write transforms to file
for i,j in zip(transforms_sym,jacobians_sym):
print "T=",sympy.pprint(i)
print "J=",sympy.pprint(j)
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):
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, disable_ft=False):
self.urdf = URDF.from_parameter_server()
self.overhead_vision_client = actionlib.SimpleActionClient(
"recognize_objects", ObjectRecognitionAction)
self.execute_trajectory_action = actionlib.SimpleActionClient(
"execute_trajectory", ExecuteTrajectoryAction)
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = controller_commander_pkg.ControllerCommander(
)
self.state = 'init'
self.current_target = None
self.current_payload = None
self.available_payloads = {
'leeward_mid_panel': 'leeward_mid_panel',
'leeward_tip_panel': 'leeward_tip_panel'
}
self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
self.speed_scalar = 1.0
self.disable_ft = disable_ft
self.tf_listener = PayloadTransformListener()
self._process_state_pub = rospy.Publisher("process_state",
ProcessState,
queue_size=100,
latch=True)
self.publish_process_state()
self.update_payload_pose_srv = rospy.ServiceProxy(
"update_payload_pose", UpdatePayloadPose)
self.get_payload_array_srv = rospy.ServiceProxy(
"get_payload_array", GetPayloadArray)
self.plan_dictionary = {}
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):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.callback)
# Publishes desired joint velocities
self.pub_vel = rospy.Publisher("/joint_velocities",
JointState,
queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
#Create "Interactive Marker" that we can manipulate in RViz
self.init_marker()
self.ee_tracking = 0
self.red_tracking = 0
self.q_current = []
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.mutex = Lock()
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def __init__(self, joint_names_vector, inactive_joint_names, js_pos):
self.robot = URDF.from_parameter_server()
self.tree, self.segment_map, self.segment_parent_map, self.segment_name_id_map = self.kdl_tree_from_urdf_model(
self.robot, inactive_joint_names, js_pos)
self.segment_id_name_map = {}
for seg_name in self.segment_name_id_map:
seg_id = self.segment_name_id_map[seg_name]
self.segment_id_name_map[seg_id] = seg_name
self.fk_solvers = {}
self.createSegmentToJointMap(joint_names_vector, inactive_joint_names)
joint_limit_map = {}
for j in self.robot.joints:
if j.limit != None:
joint_limit_map[j.name] = j.limit
self.lim_lower = np.empty(len(joint_names_vector))
self.lim_lower_soft = np.empty(len(joint_names_vector))
self.lim_upper = np.empty(len(joint_names_vector))
self.lim_upper_soft = np.empty(len(joint_names_vector))
q_idx = 0
for joint_name in joint_names_vector:
self.lim_lower[q_idx] = joint_limit_map[joint_name].lower
self.lim_lower_soft[
q_idx] = self.lim_lower[q_idx] + 15.0 / 180.0 * math.pi
self.lim_upper[q_idx] = joint_limit_map[joint_name].upper
self.lim_upper_soft[
q_idx] = self.lim_upper[q_idx] - 15.0 / 180.0 * math.pi
q_idx += 1
def __init__(self ,urdf=None):
if urdf is None:
print 'FROM PARAM SERVER'
self._youbot = URDF.from_parameter_server(key='robot_description')
else:
print 'FROM STRING'
self._youbot = URDF.from_xml_string(urdf)
self._kdl_tree = kdl_tree_from_urdf_model(self._youbot)
self._base_link = 'arm_link_0'
print "ROOT : ",self._base_link
self._tip_link = 'arm_link_5'
print "self._tip_link : ", self._tip_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 = youbot_interface.Limb(limb)
#self._joint_names = self._limb_interface.joint_names()
self._joint_names = ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5']
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):
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
#Subscribes to command for end-effector pose
rospy.Subscriber("/cartesian_command", Transform, self.command_callback)
#Subscribes to command for redundant dof
rospy.Subscriber("/redundancy_command", Float32, self.redundancy_callback)
# Publishes desired joint velocities
self.pub_vel = rospy.Publisher("/joint_velocities", JointState, queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.q_current = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.last_command_time = 0
self.last_red_command_time = 0
# Initialize timer that will trigger callbacks
self.mutex = Lock()
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def __init__(self):
#Create a tf listener
self.tfListener = tf.TransformListener()
#tf.TransfromListener is a subclass that subscribes to the "/tf" message topic and adds transform
#data to the tf data structure. As a result the object is up to date with all current transforms
#Create a publisher to a new topic name called "stylus_to_base_transf with a message type Twist"
self.Tsb = rospy.Publisher('stylus_to_base_transf',Twist)
#Find the omni from the parameter server
self.omni = URDF.from_parameter_server()
#Note: A node that initializes and runs the Phantom Omni has to be running in the background for
# from_parameter_server to to find the parameter.
# Older versions of URDF label the function "load_from_parameter_server"
#Subscribe to the "omni1_joint_states" topic, which provides the joint states measured by the omni in a
# ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the
#callback function self.get_joint_states, which is defined below.
self.joint_state_sub = rospy.Subscriber("omni1_joint_states", JointState, self.get_joint_states)
#OmniMiniProjTimers
#Run a timer to manipulate the class structure as needed. The timer's
#callback function "timer_callback" then calls the desired functions
self.timer1 = rospy.Timer(rospy.Duration(0.01), self.timer_callback)
#Create a separate slower timer on a lower frequency for a comfortable print out
self.print_timer = rospy.Timer(rospy.Duration(1), self.print_output)
return
def __init__(self):
rospy.init_node('com_calculation', anonymous=True)
self.base_link_frame = rospy.get_param("~base_link_frame",
"base_link_frame")
self.tf_prefix = rospy.get_param("~tf_prefix", "")
self.Mass = 0
#get robot description from URDF
robot = URDF.from_parameter_server()
self.links = robot.link_map
#Delete links, which contain no mass description
unnecessary_links = []
for link in self.links:
if self.links[link].inertial == None:
unnecessary_links.append(link)
for link in unnecessary_links:
del self.links[link]
#Calculate the total mass of the robot
for link in self.links:
self.Mass += self.links[link].inertial.mass
rospy.loginfo("Mass of robot is %f", self.Mass)
self.calculator()
def callback(joint_values):
#rospy.loginfo("this is joint_values.name")
#rospy.loginfo(joint_values.name)
robot = URDF.from_parameter_server()
link_names = []
joints = []
joints_names = []
link_name = robot.get_root()
while True:
if link_name not in robot.child_map:
break
(next_joint_name, next_link_name) = robot.child_map[link_name][0]
if next_joint_name not in robot.joint_map:
break
joints.append(robot.joint_map[next_joint_name])
link_names.append(next_link_name)
joints_names.append(next_joint_name)
link_name = next_link_name
#rospy.loginfo("this is joints_names")
#rospy.loginfo(joints_names)
calculate_and_publish_transforms(link_names, joints, joint_values)
def __init__(self):
#Loads the robot model, which contains the robot's kinematics information
self.num_joints = 0
self.joint_names = []
self.joint_axes = []
self.robot = URDF.from_parameter_server()
self.base = self.robot.get_root()
self.get_joint_info()
# Wait for moveit IK service
rospy.wait_for_service("compute_ik")
self.ik_service = rospy.ServiceProxy('compute_ik', moveit_msgs.srv.GetPositionIK)
print "IK service ready"
# Wait for validity check service
rospy.wait_for_service("check_state_validity")
self.state_valid_service = rospy.ServiceProxy('check_state_validity',
moveit_msgs.srv.GetStateValidity)
print "State validity service ready"
# MoveIt parameter
robot_moveit = moveit_commander.RobotCommander()
self.group_name = robot_moveit.get_group_names()[0]
#Subscribe to topics
rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
rospy.Subscriber('/motion_planning_goal', Transform, self.motion_planning)
self.current_obstacle = "None"
rospy.Subscriber('/obstacle', String, self.get_obstacle)
#Set up publisher
self.pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)
def __init__(self, use_moveit=False):
self.use_moveit = use_moveit
self.baxter = URDF.from_parameter_server(key='robot_description')
self.kdl_tree = kdl_tree_from_urdf_model(self.baxter)
self.base_link = self.baxter.get_root()
self.right_limb_interface = baxter_interface.Limb('right')
self.left_limb_interface = baxter_interface.Limb('left')
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState)
self.get_model_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)
# Listen to collision information
# self.collision_getter = InfoGetter()
# self.collision_topic = "/hug_collision"
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
if not self._init_state:
print("Enabling robot... ")
self._rs.enable()
if self.use_moveit:
# Moveit group setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_python.PlanningSceneInterface(self.robot.get_planning_frame())
# self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "right_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
# ######################## Create Hugging target #############################
# humanoid hugging target # remember to make sure target_line correct + - y
self.target_pos_start = np.asarray([0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame
self.target_line_start = np.empty([22, 3], float)
for i in range(11):
self.target_line_start[i] = self.target_pos_start + [0, -0.0, 1.8] - (asarray([0, -0.0, 1.8]) - asarray([0, -0.0, 0.3]))/10*i
self.target_line_start[i+11] = self.target_pos_start + [0, -0.5, 1.3] + (asarray([0, 0.5, 1.3]) - asarray([0, -0.5, 1.3]))/10*i
self.target_line = self.target_line_start
# Build line point graph for interaction mesh
# reset right arm
start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t01 = threading.Thread(target=resetarm_job, args=(self.right_limb_interface, start_point_right))
t01.start()
# reset left arm
start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t02 = threading.Thread(target=resetarm_job, args=(self.left_limb_interface, start_point_left))
t02.start()
t02.join()
t01.join()
right_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.right_limb_interface, 'right')
left_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.left_limb_interface, 'left')
graph_points = np.concatenate((right_limb_pose[5:], left_limb_pose[5:], self.target_line_start), 0)
self.triangulation = Delaunay(graph_points)
def main():
parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file',
type=argparse.FileType('r'),
nargs='?',
default=None,
help='File to load. Use - for stdin')
parser.add_argument('-o',
'--output',
type=argparse.FileType('w'),
default=None,
help='Dump file to XML')
args = parser.parse_args()
if args.file is None:
print 'FROM PARAM SERVER'
robot = URDF.from_parameter_server()
else:
print 'FROM STRING'
robot = URDF.from_xml_string(args.file.read())
print(robot)
if args.output is not None:
args.output.write(robot.to_xml_string())
def __init__(self):
self.robot = URDF.from_parameter_server()
self.num_joints = 0
self.joint_names = []
self.joint_axes = []
self.joint_transforms = []
self.x_current = tf.transformations.identity_matrix()
self.get_joint_info()
self.current_joint_state = JointState()
rospy.Subscriber("joint_states", JointState, self.js_callback)
# Wait for validity check service
rospy.wait_for_service("check_state_validity")
self.state_valid_service = rospy.ServiceProxy(
'check_state_validity', moveit_msgs.srv.GetStateValidity)
print "State validity service ready"
# MoveIt parameter
self.group_name = "lwr_arm"
rospy.Subscriber("/motion_planning_goal", Transform, self.mp_callback)
self.pub_trajectory = rospy.Publisher("/joint_trajectory",
JointTrajectory,
queue_size=1)
def execute_move(self, pos):
rospy.loginfo('moving')
# Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block
pos.position.z += .1
pos.position.x += .005
q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
p =[[pos.position.x],[pos.position.y],[pos.position.z]]
# Convert quaternion data to rotation matrix
R = quat_to_so3(q);
# Form transformation matrix
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
# Create seed with current position
q0 = kdl_kin.random_joint_angles()
limb_interface = baxter_interface.limb.Limb('right')
limb_interface.set_joint_position_speed(.3)
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
pose = kdl_kin.forward(q0)
pose[0:3,0:3] = R
pose[0:3,3] = p
# Solve for joint angles, iterating if no solution is found
seed = 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
while q_ik == None:
seed += 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
rospy.loginfo(q_ik)
# Calculate the joint trajectory with a quintic time scaling
q_list = JointTrajectory(q0,q_ik,1,50,5)
# Iterate through list
for q in q_list:
# Format joint angles as limb joint angle assignment
angles = limb_interface.joint_angles()
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
rospy.sleep(.07)
# Send joint move command
limb_interface.set_joint_positions(angles)
# Publish state and hand position
rospy.sleep(1)
rospy.loginfo(4)
self._pub_state.publish(4)
rospy.loginfo(pos)
self._pub_hand.publish(pos)
self._done = True
print('Done')
def execute_cb(self,goal):
self.moving_to_new_x_pos = False
self.moving_to_new_y_pos = False
self.stop_base_movement = False
self.max_virtual_x_vel = 0.05
self.max_virtual_y_vel = 0.05
self.commanded_virtual_x_pos = 0.0
self.commanded_virtual_y_pos = 0.0
self.commanded_virtual_x_vel = 0.0
self.commanded_virtual_y_vel = 0.0
self.virtual_x_cmd_time_sec = 0.0
self.virtual_y_cmd_time_sec = 0.0
self.virtual_x_running_time_sec = 0.0
self.virtual_y_running_time_sec = 0.0
youbot_urdf = URDF.from_parameter_server()
self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link")
self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link")
# Create a timer that will be used to monitor the velocity of the virtual
# joints when we need them to be positioning themselves.
self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback)
self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10)
self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10)
self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Give the publishers time to get setup before trying to do any actual work.
rospy.sleep(2)
# Initialize at the candle position.
self.publish_arm_joint_positions(armJointPosCandle)
rospy.sleep(2.0)
#self.publish_arm_joint_positions(armJointPos1)
#rospy.sleep(3.0)
#self.publish_arm_joint_positions(armJointPos2)
#rospy.sleep(10.0)
#self.publish_arm_joint_positions(armJointPosCandle)
#rospy.sleep(2.0)
# Go through the routine of picking up the block.
self.grasp_routine()
self._result.successOrNot=1
self._as.set_succeeded(self._result)
def __init__(self):
"""Announces that it will publish forward kinematics results, in the form of tfMessages.
"tf" stands for "transform library", it's ROS's way of communicating around information
about where things are in the world"""
self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage, queue_size=1)
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.callback)
def main():
a = rospy.init_node('set_base_frames')
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
if mtmr_robot is None:
mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name)
# pdb.set_trace()
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1)
ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip
ecm_tip = np.linalg.inv(ecm_tip)
psm1_base_frame = psm1_kin.forward([])
psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame)
psm2_base_frame = psm2_kin.forward([])
psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame)
psm1_pub.publish(psm1_message)
psm2_pub.publish(psm2_message)
# mtmr_pub.publish(message)
print (psm1_message)
print (psm2_message)
sleep (1)
def _wait_and_get_robot(self):
t_init = rospy.Time.now()
t_timeout = rospy.Duration(5) # 10 seconds
while rospy.Time.now() - t_init < t_timeout:
try:
robot = URDF.from_parameter_server("/mitsubishi_arm/robot_description")
print "*** Obtained robot_description ***"
return robot
except KeyError:
print "Key error."
rospy.sleep(rospy.Duration(1))
raise KeyError("robot_description was not found")
def __init__(self):
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.ecm_msg = None
self.__clutchNGo_mode__ = self.MODE.simulation
self.autocamera = Autocamera()
def find_feasible_release(catch_x, catch_y, catch_z, pos):
found = False;
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
while not found:
X, th_init, throw_y, throw_z, vel, alpha = sample_state(catch_x, catch_y, catch_z, pos)
ik_test, q_ik = test_for_ik(X, th_init, kdl_kin)
if ik_test:
if test_joint_vel(q_ik, vel, alpha, kdl_kin):
found = True
print q_ik
return throw_y, throw_z, vel, alpha
def move_to(pos):
pub_demo_state = rospy.Publisher('demo_state',Int16, queue_size = 10)
if (pos == 1):
catch = np.array([.65, .2, 0]) # my .7?
R = np.array([[ 0.26397895, -0.34002068, 0.90260791],
[-0.01747676, -0.93733484, -0.34799134],
[ 0.96437009, 0.07608772, -0.25337913]])
elif (pos == 2):
catch = np.array([.68, -.05, 0])
R = np.array([[0,0,1],[0,-1,0],[1,0,0]])
elif (pos == 3):
catch = np.array([.72, .1, 0])
R = np.array([[ 0.26397895, -0.34002068, 0.90260791],
[-0.01747676, -0.93733484, -0.34799134],
[ 0.96437009, 0.07608772, -0.25337913]])
else:
pass
th_init = np.array([-.3048, -.2703, -.1848, 1.908, .758, -1.234, -3.04])
X = RpToTrans(R,catch)
# Find end joint angles with IK
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'left_gripper_base')
seed = 0
q_ik = kdl_kin.inverse(X, th_init)
while q_ik == None:
seed += 0.01
q_ik = kdl_kin.inverse(X, th_init+seed)
if (seed>1):
# return False
break
q_goal = q_ik
print q_goal
limb_interface = baxter_interface.limb.Limb('left')
angles = limb_interface.joint_angles()
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q_goal[ind]
limb_interface.move_to_joint_positions(angles)
# rospy.sleep(5)
print 'done'
pub_demo_state.publish(1)
def __init__(self, side, step, x_min, x_max, y_min, y_max, z_min, z_max, pt_c_in_T2, min_dist, max_dist, rot):
self.min_dist2 = min_dist*min_dist
self.max_dist2 = max_dist*max_dist
self.x_set = list(np.arange(x_min, x_max, step))
self.y_set = list(np.arange(y_min, y_max, step))
if z_min > z_max:
self.z_set = list(np.arange(z_min, z_max, -step))
else:
self.z_set = list(np.arange(z_min, z_max, step))
self.pt_c_in_T2 = pt_c_in_T2
self.rot = rot
self.robot = URDF.from_parameter_server()
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain("torso_link2", side + "_HandPalmLink")
self.q_min = PyKDL.JntArray(7)
self.q_max = PyKDL.JntArray(7)
self.q_limit = 0.01
self.q_min[0] = -2.96 + self.q_limit
self.q_min[1] = -2.09 + self.q_limit
self.q_min[2] = -2.96 + self.q_limit
# self.q_min[3] = -2.09 + self.q_limit
self.q_min[3] = 15.0/180.0*math.pi
self.q_min[4] = -2.96 + self.q_limit
self.q_min[5] = -2.09 + self.q_limit
self.q_min[6] = -2.96 + self.q_limit
self.q_max[0] = 2.96 - self.q_limit
# self.q_max[1] = 2.09 - self.q_limit
self.q_max[1] = -15.0/180.0*math.pi
self.q_max[2] = 2.96 - self.q_limit
self.q_max[3] = 2.09 - self.q_limit
self.q_max[4] = 2.96 - self.q_limit
# self.q_max[5] = 2.09 - self.q_limit
self.q_max[5] = -15.0/180.0*math.pi
self.q_max[6] = 2.96 - self.q_limit
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.chain)
self.vel_ik_solver = PyKDL.ChainIkSolverVel_pinv(self.chain)
self.ik_solver = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.q_min, self.q_max, self.fk_solver, self.vel_ik_solver, 100)
self.q_out = PyKDL.JntArray(7)
self.singularity_angle = 15.0/180.0*math.pi
def main():
parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
default=None, help='File to load. Use - for stdin')
parser.add_argument('-o', '--output', type=argparse.FileType('w'),
default=None, help='Dump file to XML')
args = parser.parse_args()
if args.file is None:
print 'FROM PARAM SERVER'
robot = URDF.from_parameter_server()
else:
print 'FROM STRING'
robot = URDF.from_xml_string(args.file.read())
print(robot)
if args.output is not None:
args.output.write(robot.to_xml_string())
def __init__(self):
self.fk_solver = velma_fk_ik.VelmaFkIkSolver()
self.robot = URDF.from_parameter_server()
self.mimic_joints_map = {}
self.joint_name_limit_map = {}
joint_idx_ros = 0
for i in range(len(self.robot.joints)):
joint = self.robot.joints[i]
if joint.joint_type == "fixed":
continue
if joint.mimic != None:
self.mimic_joints_map[joint.name] = joint.mimic
self.joint_name_limit_map[joint.name] = joint.limit
joint_idx_ros += 1
def __init__(self):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6']
#To be reached 1 second after starting along the trajectory
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=[0]*6
trajectory_point.time_from_start=rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb)
def __init__(self, iksvc, limb):
self.centroid = None
#self.x_extremes = None
self.axes = None
self.ir_reading = None
self.iksvc = iksvc
self.limb = limb
self.limb_iface = baxter_interface.Limb(limb)
self.gripper_if = baxter_interface.Gripper(limb)
self.tf_listener = tf.TransformListener()
self.stateidx = 0
self.states = self.wait_centroid, self.orient, self.servo_xy, self.servo_z, self.grip_state, self.done_state
self.done = 0
self.sign = 1
#Check if we are exceeding the joint limit specified by robot_urdf
robot = URDF.from_parameter_server()
key = limb+"_w2"
for joint in robot.joints:
if joint.name == key:
break
self.wristlim = joint.limit
"""paramnames = ["servo_speed", "min_pose_z", "min_ir_depth"]
paramvals = []
for param in paramnames:
topic = "/servo_to_object/"
paramvals.append(rospy.get_param(topic+param))
self.inc, self.min_pose_z, self.min_ir_depth = tuple(paramvals)
self.goal_pos = (rospy.get_param(topic+"camera_x")*float(rospy.get_param(topic+"goal_ratio_x")), rospy.get_param(topic+"camera_y")*float(rospy.get_param(topic+"goal_ratio_y")))
self.grip_height = self.min_pose_z"""
self.inc = params['servo_speed']
self.angle_inc = params['angle_inc']
self.min_pose_z = params['min_pose_z']
self.min_ir_depth = params['min_ir_depth']
self.goal_pos = (params['camera_x']*params['goal_ratio_x'],\
params['camera_y']*params['goal_ratio_y'])
def callback(msgs):
"msgs = ContactStatesStamped"
global g_config
if g_config.use_parent_link:
urdf_robot = URDF.from_parameter_server()
marker_array = MarkerArray()
for msg, i in zip(msgs.states, range(len(msgs.states))):
marker = Marker()
link_name = msg.header.frame_id
if g_config.use_parent_link:
# lookup parent link
chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name)
link_name = chain[-3]
mesh_file, offset = find_mesh(link_name)
marker.header.frame_id = link_name
marker.header.stamp = rospy.Time.now()
marker.type = Marker.MESH_RESOURCE
if msg.state.state == ContactState.ON:
marker.color.a = g_config.on_alpha
marker.color.r = g_config.on_red
marker.color.g = g_config.on_green
marker.color.b = g_config.on_blue
elif msg.state.state == ContactState.OFF:
marker.color.a = g_config.off_alpha
marker.color.r = g_config.off_red
marker.color.g = g_config.off_green
marker.color.b = g_config.off_blue
marker.scale.x = g_config.marker_scale
marker.scale.y = g_config.marker_scale
marker.scale.z = g_config.marker_scale
marker.pose = offset
marker.mesh_resource = mesh_file
marker.frame_locked = True
marker.id = i
if msg.state.state == ContactState.OFF:
if not g_config.visualize_off:
marker.action = Marker.DELETE
marker_array.markers.append(marker)
pub.publish(marker_array)
def __init__(self, limb):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._base_link = self._baxter.get_root()
self._tip_link = limb + '_gripper'
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._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():
rospy.init_node('set_base_frames')
sleep (1)
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
# pdb.set_trace()
mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1)
ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1)
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1)
mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1)
mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1)
p1_base = psm1_kin.forward([]) # PSM1 Base Frame
p2_base = psm2_kin.forward([]) # PSM2 Base Frame
e_base = ecm_base.forward([]) # ECM Base Frame
e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip
camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)])
r = lambda axis, rad: rotate(axis, rad)
mtmr_m = e;# mtmr_m = mtmr_m**-1
mtml_m = e
print 'qmat'
qmsg = Quaternion()
temp = quat_from_homomat(p1_base)
print quat_from_homomat(p1_base)
message = pose_from_homomat(p1_base);
psm1_pub.publish(message)
while not rospy.is_shutdown():
#print p1_base
#print message
psm1_pub.publish(message)
print ('sure\n')
# psm2_pub.publish(pose_from_homomat(p2_base))
# psm1_pub.publish(pose_from_homomat(e_base))
# mtml_pub.publish( pose_from_homomat(mtml_m))
# mtmr_pub.publish( pose_from_homomat(mtmr_m))
print ('\n\Hello: nmtml rotation: \n')
print( mtml_m.__repr__( ))
print(pose_from_homomat(mtml_m))
print ('\n\nmtmr rotation: \n')
print( mtmr_m.__repr__())
def execute_move(self, pos):
rospy.loginfo('moving')
# Read in pose data
q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
p =[[pos.position.x],[pos.position.y],[pos.position.z]]
# Convert quaternion data to rotation matrix
R = quat_to_so3(q);
# Form transformation matrix
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
# Create seed with current position
q0 = kdl_kin.random_joint_angles()
limb_interface = baxter_interface.limb.Limb('right')
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
pose = kdl_kin.forward(q0)
Xstart = copy(np.asarray(pose))
pose[0:3,0:3] = R
pose[0:3,3] = p
Xend = copy(np.asarray(pose))
# Compute straight-line trajectory for path
N = 500
Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5)
thList = np.empty((N,7))
thList[0] = q0;
for i in range(N-1):
# Solve for joint angles
seed = 0
q_ik = kdl_kin.inverse(Xlist[i+1], thList[i])
while q_ik == None:
seed += 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
thList[i+1] = q_ik
# rospy.loginfo(q_ik)
# # Solve for joint angles
# seed = 0.3
# q_ik = kdl_kin.inverse(pose, q0+seed)
# while q_ik == None:
# seed += 0.3
# q_ik = kdl_kin.inverse(pose, q0+seed)
# rospy.loginfo(q_ik)
# q_list = JointTrajectory(q0,q_ik,1,100,5)
# for q in q_list:
# Format joint angles as limb joint angle assignment
angles = limb_interface.joint_angles()
angle_count = 0
for ind, joint in enumerate(limb_interface.joint_names()):
# if fabs(angles[joint] - q_ik[ind]) < .05:
# angle_count += 1
angles[joint] = q_ik[ind]
# rospy.loginfo(angles)
rospy.sleep(.003)
# Send joint move command
rospy.loginfo('move to object')
# rospy.loginfo(angle_count)
# if angle_count > 4:
# nothing = True;
# else:
limb_interface.set_joint_position_speed(.3)
limb_interface.set_joint_positions(angles)
self._done = True
print('Done')
