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 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 main():
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = URDF.load_from_parameter_server(verbose=False)
else:
robot = URDF.load_xml_file(sys.argv[1], verbose=False)
tree = kdl_tree_from_urdf_model(robot)
num_non_fixed_joints = 0
for j in robot.joints:
if robot.joints[j].joint_type != 'fixed':
num_non_fixed_joints += 1
print "URDF non-fixed joints: %d;" % num_non_fixed_joints,
print "KDL joints: %d" % tree.getNrOfJoints()
print "URDF joints: %d; KDL segments: %d" %(len(robot.joints),
tree.getNrofSegments())
import random
base_link = robot.get_root()
end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
chain = tree.getChain(base_link, end_link)
print "Root link: %s; Random end link: %s" % (base_link, end_link)
for i in range(chain.getNrOfSegments()):
print chain.getSegment(i).getName()
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 __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):
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 runDiff(original_file, new_file, output_file):
original_robot = URDF.from_xml_file(original_file)
new_robot = URDF.from_xml_file(new_file)
# only joint and link are considered
diffs = dict()
for j in original_robot.joints:
new_j = findJointByName(new_robot, j.name)
# check origin difference
if new_j:
diff = jointDiff(j, new_j)
if diff:
diffs[j.name] = diff
with open(output_file, "w") as f:
f.write(yaml.dump(diffs))
print yaml.dump(diffs)
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):
#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, urdf_path, sem_path, name_suffix=None):
self.nsmap = {
"xsd": "http://www.w3.org/2001/XMLSchema#",
"owl": "http://www.w3.org/2002/07/owl#",
"xsd": "http://www.w3.org/2001/XMLSchema#",
"srdl2": "http://knowrob.org/kb/srdl2.owl#",
"owl2xml": "http://www.w3.org/2006/12/owl2-xml#",
"knowrob": "http://knowrob.org/kb/knowrob.owl#",
"rdfs": "http://www.w3.org/2000/01/rdf-schema#",
"rdf": "http://www.w3.org/1999/02/22-rdf-syntax-ns#",
"srdl2-comp": "http://knowrob.org/kb/srdl2-comp.owl#",
"srdl2-cap": "http://knowrob.org/kb/srdl2-cap.owl#",
"qudt-unit": "http://qudt.org/vocab/unit#",
}
self.imports = [
"package://knowrob_srdl/owl/srdl2-comp.owl",
"package://knowrob_common/owl/knowrob.owl",
]
self.id_gen = UniqueStringGenerator()
self.urdf = URDF.from_xml_file(urdf_path)
self.urdf.check_valid()
basename = os.path.basename(sem_path)
namespace, _ = os.path.splitext(basename)
self.map_ns = namespace
if name_suffix is None:
self.map_name = self.urdf.name + "_" + self.id_gen.gen()
else:
self.map_name = self.urdf.name + "_" + name_suffix
self.map_uri_base = "http://knowrob.org/kb/%s" % basename
self.map_uri = self.map_uri_base + "#"
self.nsmap[self.map_ns] = self.map_uri
self.transformations = {}
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):
rospy.init_node('hybrid_node')
self.freq = 100
self.rate = rospy.Rate(self.freq) # 100 hz
# pub
self.pub_test = rospy.Publisher('/hybrid/test', String)
# sub
self.sub_jr3 = rospy.Subscriber('/jr3/wrench', WrenchStamped, self.cb_jr3)
self.sub_joy = rospy.Subscriber('/spacenav/joy', Joy, self.cb_joy)
self.sub_enable = rospy.Subscriber('/hybrid/enable', Bool, self.cb_enable)
self.sub_cmd = rospy.Subscriber('/hybrid/cmd', String, self.cb_cmd)
# tf
self.ler = tf.TransformListener() # listener
self.ber = tf.TransformBroadcaster() # broadcaster
# datas
self.enabled = False
self.cmdfrm = Frame()
self.wrench = Wrench()
self.cmdtwist = Twist()
self.urdf = rospy.get_param('/wam/robot_description')
print self.urdf
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.chain = self.robot.get_chain('base_link',
'cutter_tip_link')
print self.chain
pass
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, 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()
#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):
#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 __init__(self):
super(RobotHeadWidget, self).__init__()
self.lock = Lock()
try:
robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
except Exception, e:
self.showError("Failed to load /robot_description: " + e.message)
return
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3],
T[1, 3],
T[2, 3],
math.atan2(T[1, 0], T[0, 0]),
-math.asin(T[2, 0])])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop,))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
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 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 main():
rospy.init_node("joint_kinematics")
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = URDF.load_from_parameter_server(verbose=False)
else:
robot = URDF.load_xml_file(sys.argv[1], verbose=False)
if True:
import random
base_link = robot.get_root()
end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
print "Root link: %s; Random end link: %s" % (base_link, end_link)
js_kin = JointKinematics(robot, base_link, end_link)
print "Joint angles:", js_kin.get_joint_angles()
print "Joint angles (wrapped):", js_kin.get_joint_angles(True)
print "Joint velocities:", js_kin.get_joint_velocities()
print "Joint efforts:", js_kin.get_joint_efforts()
print "Jacobian:", js_kin.jacobian()
kdl_pose = js_kin.forward()
print "FK:", kdl_pose
print "End effector force:", js_kin.end_effector_force()
if True:
import tf
from hrl_geom.pose_converter import PoseConv
tf_list = tf.TransformListener()
rospy.sleep(1)
t = tf_list.getLatestCommonTime(base_link, end_link)
tf_pose = PoseConv.to_homo_mat(tf_list.lookupTransform(base_link, end_link, t))
print "Error with TF:", np.linalg.norm(kdl_pose - tf_pose)
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 create_joint_kin(base_link,
end_link,
urdf_filename=None,
timeout=1.,
wait=False,
description_param="robot_description"):
if urdf_filename is None:
robot = URDF.from_parameter_server(key=description_param)
else:
f = file(urdf_filename, 'r')
robot = Robot.from_xml_string(f.read())
f.close()
if not wait:
return JointKinematics(robot, base_link, end_link, timeout=timeout)
else:
return JointKinematicsWait(robot, base_link, end_link)
def runPatch(input_file, patch_yaml, output_file):
input_robot = URDF.from_xml_file(input_file)
patch_param = yaml.load(open(patch_yaml))
for joint_name in patch_param.keys():
diff = patch_param[joint_name]
if diff.has_key("xyz"):
j = input_robot.joint_map[joint_name]
j.origin.xyz = diff["xyz"]
if diff.has_key("rpy"):
j = input_robot.joint_map[joint_name]
j.origin.rpy = diff["rpy"]
if output_file:
with open(output_file, "w") as f:
f.write(input_robot.to_xml_string())
else:
print input_robot.to_xml_string()
def joints_limits_from_urdf(self):
robot_urdf = URDF.from_parameter_server()
self.joints_limits = []
for joint in robot_urdf.joints:
if joint.name in self.joint_name_list:
self.joints_limits.append(joint.limit)
self.joints_limits_dict[joint.name] = joint.limit
rospy.set_param(
"/niryo_robot/robot_command_validation/joint_limits/{}".
format(joint.name), {
"min": joint.limit.lower,
"max": joint.limit.upper
})
return self.joints_limits
def runPatch(input_file, patch_yaml, output_file):
input_robot = URDF.from_xml_file(input_file)
patch_param = yaml.load(open(patch_yaml))
for joint_name in patch_param.keys():
diff = patch_param[joint_name]
if "xyz" in diff:
j = input_robot.joint_map[joint_name]
j.origin.xyz = diff["xyz"]
if "rpy" in diff:
j = input_robot.joint_map[joint_name]
j.origin.rpy = diff["rpy"]
if output_file:
with open(output_file, "w") as f:
f.write(input_robot.to_xml_string())
else:
print(input_robot.to_xml_string())
def __init__(self):
print("MotionTaskWithConstraint is initialized !")
self._giskard = GiskardWrapper()
rospy.logout("- Set pose kitchen")
kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
kitchen_pose.header.frame_id = "map"
# setup kitchen
rospy.logout("- Get urdf")
self.urdf = rospy.get_param("kitchen_description")
self.parsed_urdf = URDF.from_xml_string(self.urdf)
self.config_file = None
rospy.logout("- clear urdf and add kitchen urdf")
self._giskard.clear_world()
self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
def get_jacabian_from_joint(self, urdfname, jointq, flag):
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
chain = tree.getChain("base_link", "ee_link")
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
q = jointq
pose = kdl_kin.forward(q)
q0 = Kinematic()
if flag == 1:
q_ik = q0.best_sol_for_other_py([1.] * 6, 0, q0.Forward(q))
else:
q_ik = kdl_kin.inverse(pose)
if q_ik is not None:
pose_sol = kdl_kin.forward(q_ik)
J = kdl_kin.jacobian(q)
return J, pose
def initialize_kinematic_solvers(self):
robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
self._robot = URDF.from_parameter_server(key=robot_description)
self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
# self._base_link = self._robot.get_root()
self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
self._ee_frame = PyKDL.Frame()
self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
def 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 __init__(self):
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()
self._tip_link = 'left_hand_palm_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 from_urdf(cls, urdf_string, transmission_suffix='_thruster_transmission'):
'''
Load from an URDF string. Expects each thruster to be connected a transmission ending in the specified suffix.
A transform between the propeller joint and base_link must be available
'''
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
joints = []
positions = []
angles = []
limit = -1
ratio = -1
for transmission in urdf.transmissions:
find = transmission.name.find(transmission_suffix)
if find != -1 and find + len(transmission_suffix) == len(transmission.name):
if len(transmission.joints) != 1:
raise Exception('Transmission {} does not have 1 joint'.format(transmission.name))
if len(transmission.actuators) != 1:
raise Exception('Transmission {} does not have 1 actuator'.format(transmission.name))
t_ratio = transmission.actuators[0].mechanicalReduction
if ratio != -1 and ratio != t_ratio:
raise Exception('Transmission {} has a different reduction ratio (not supported)'.format(t_ratio))
ratio = t_ratio
joint = None
for t_joint in urdf.joints:
if t_joint.name == transmission.joints[0].name:
joint = t_joint
if joint is None:
rospy.logerr('Transmission joint {} not found'.format(transmission.joints[0].name))
try:
trans = buff.lookup_transform('base_link', joint.child, rospy.Time(), rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(transmission.name[0:find])
positions.append(translation[0:2])
angles.append(yaw)
joints.append(joint.name)
if limit != -1 and joint.limit.effort != limit:
raise Exception('Thruster {} had a different limit, cannot proceed'.format(joint.name))
limit = joint.limit.effort
return cls(names, positions, angles, ratio, limit, joints=joints)
def __init__(self):
self.mutex = Lock()
self.start = False
foo = CartesianCommand()
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
print('start')
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.callback)
# Publishes Cartesian goals
self.pub_command = rospy.Publisher("/cartesian_command",
CartesianCommand,
queue_size=1)
# Publishes IK command
self.ik_command = rospy.Publisher("/ik_command",
geometry_msgs.msg.Transform,
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.cc_mode = True
print('self.robot', self.robot)
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.q0_base = 0
self.angle_base = 0
self.delta = 0
self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback)
print('self.ee_tracking', self.ee_tracking)
self.start = True
def __init__(self):
super(QWidget, self).__init__()
self.hand_prefix = rospy.get_param("~hand_name", "")
self.display_joints = rospy.get_param("~visible_joints", [])
self.joint_wild_card = rospy.get_param("~show_all_joints_with",
"phand_")
self.only_hand_joints = rospy.get_param("~only_hand_joints", True)
self.not_display_joints = [
self.hand_prefix + 'rightcylinder_rod',
self.hand_prefix + 'wristBase_cylinderR',
self.hand_prefix + 'horizontal_R_vertical_R',
self.hand_prefix + 'leftcylinder_rod',
self.hand_prefix + 'wristBase_horizontal_L',
self.hand_prefix + 'horizontal_L_vertical_L',
self.hand_prefix + 'index_deviation',
self.hand_prefix + 'index_cylinder_base',
]
self.gridLayout = QGridLayout(self)
self.setWindowTitle(self.hand_prefix + "joint publisher")
self.show()
self.robot = URDF.from_parameter_server()
self.joint_state = JointState()
self.jc = JointCalculations()
self.display_joints.extend([
self.hand_prefix + 'rotation_x', self.hand_prefix + 'rotation_y',
self.hand_prefix + 'cylinder_rod'
])
self.joint_state.name = []
self.generate_joint_list()
self.joint_state.position = [0] * len(self.joint_state.name)
self.sliders = []
self.setup_gui()
self.publish_joint_state = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self.publish_joint_state_timer = QTimer()
self.publish_joint_state_timer.timeout.connect(self.update_joint_state)
self.publish_joint_state_timer.start(100)
def __init__(self, limb):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._right_limb_interface = baxter_interface.Limb('right')
self._base_link = self._baxter.get_root()
self._tip_link = limb + '_gripper'
self.solver = KDLKinematics(self._baxter, self._base_link,
self._tip_link)
self.rs = RobotState()
self.rs.joint_state.name = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
self.rs.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# 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)
# service
self.set_model_config = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
GetLinkState)
self.sv_srv = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
# human target
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
] - (np.asarray([0, -0.0, 1.8]) -
np.asarray([0, -0.0, 0.5])) / 10 * i
self.target_line_start[i + 11] = self.target_pos_start + [
0, -0.5, 1.3
] + (np.asarray([0, 0.5, 1.3]) -
np.asarray([0, -0.5, 1.3])) / 10 * i
self.target_line = self.target_line_start
def __init__(self):
"""
Parses the parameter server to extract the necessary information.
"""
if not rospy.has_param("arm"):
rospy.logerr("No arm param defined.")
arm_parameters = {'joint_prefix': {}, 'mapping': {}}
else:
arm_parameters = rospy.get_param("arm")
# TODO(@anyone): This parameter is never set. This script needs to be modified to find available
# arms from robot_description and present them appropriately. (sr_core issue #74)
self.arm_config = ArmConfig(arm_parameters["mapping"],
arm_parameters["joint_prefix"])
self.arm_joints = {}
if rospy.has_param('robot_description'):
robot_description = rospy.get_param('robot_description')
robot_urdf = URDF.from_xml_string(robot_description)
hand_default_joints = HandJoints.get_default_joints()
possible_arm_joints = []
for joint in robot_urdf.joints:
if joint.type != 'fixed' and joint.name not in hand_default_joints:
match_suffix = False
for hand_default_joint_name in hand_default_joints:
if joint.name.endswith('_' + hand_default_joint_name):
match_suffix = True
break
if not match_suffix:
possible_arm_joints.append(joint.name)
for arm_id, arm_mapping in self.arm_config.mapping.items():
self.arm_joints[arm_mapping] = []
arm_joint_prefix = self.arm_config.joint_prefix[arm_id]
for joint_name in possible_arm_joints:
if joint_name.startswith(arm_joint_prefix):
self.arm_joints[arm_mapping].append(joint_name)
else:
rospy.logwarn(
"No robot_description found on parameter server. Assuming that there is no arm."
)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo', anonymous=True)
rospy.Subscriber('/joint_states',
JointState,
self.obtain_joint_states_sim,
queue_size=1)
self.arm = moveit_commander.MoveGroupCommander('manipulator_i5')
self.robot = URDF.from_xml_file(
"/home/zy/catkin_ws/src/aubo_robot-master/aubo_description/urdf/aubo_i5.urdf"
)
self.aubo_q = [0, pi / 2, pi / 2, pi / 2, 0, 0]
self.aubo_q1 = np.matrix(self.aubo_q)
self.kp = 50
self.rdot_dsr = np.matrix([0.0, 0.0, 0.0])
self.rate = rospy.Rate(10) # 10hz
def __init__(self):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.seam_point_dict = {}
self.plan_seam_dict = {}
self.speed = 1.0
self.display = True
robot_urdf = URDF.from_parameter_server()
#robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_kin = KDLKinematics(robot_urdf, "base_link", "tool0")
self.move_group = moveit_commander.MoveGroupCommander("GP12_Planner")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = self.move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)
self.dxf_file_response_pub = rospy.Publisher('file_response_string',
std_msgs.msg.String,
queue_size=10)
self.plan_response_pub = rospy.Publisher('plan_response_string',
std_msgs.msg.String,
queue_size=10)
self.execute_seam_response_pub = rospy.Publisher(
'execute_seam_response_string', std_msgs.msg.String, queue_size=10)
self.plan_and_execute_response_pub = rospy.Publisher(
'plan_and_execute_seam_response_string',
std_msgs.msg.String,
queue_size=10)
self.move_to_stream_start_response_pub = rospy.Publisher(
'move_to_seam_response', std_msgs.msg.String, queue_size=10)
#Ros message used to pass dxf file name to node and process dxf file
rospy.Subscriber("dxf_file_name", String, self.dxf_grabber_readfile)
#Ros message used to plan and view motion before returning
rospy.Subscriber("plan_seam_motion", String,
self.plan_robot_motion_call)
rospy.Subscriber("execute_seam_motion", String,
self.execute_robot_motion_call)
rospy.Subscriber("plan_and_execute_seam", String,
self.plan_and_execute_call)
rospy.Subscriber("move_to_seam_start", String,
self.move_to_seam_start_call)
self.followjointaction = rospy.Publisher(
"joint_trajectory_action/goal",
control_msgs.msg.FollowJointTrajectoryActionGoal,
queue_size=10)
def do(self):
self.robot = URDF.from_xml_file(self.file)
for func_name in plugins.functions.keys():
if getattr(self, func_name) is not None:
break
for plugin in plugins.loaded.values():
if plugin.name in getattr(self, func_name):
func = getattr(plugin, func_name)
if func(self.robot, self.file):
# if function returned True that means file changed in place, we need re read it
self.read_robot_from_file()
else:
# write changes to disk
self.write_robot_to_file()
self.write_robot_to_file()
def get_jacabian_from_joint(self, urdfname, jointq, flag):
#robot = URDF.from_xml_file("/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf")
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
# print tree.getNrOfSegments()
chain = tree.getChain("base_link", "tool0")
# print chain.getNrOfJoints()
# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "base_link", "tool0")
q = jointq
#q = [0, 0, 1, 0, 1, 0]
pose = kdl_kin.forward(
q) # forward kinematics (returns homogeneous 4x4 matrix)
J = kdl_kin.jacobian(q)
#print 'J:', J
return J, pose
def configure_robot(self, description):
self.get_logger().info('Got description, configuring robot')
# Load description
# From https://github.com/ros-controls/ros_controllers/blob/noetic-devel/diff_drive_controller/src/diff_drive_controller.cpp
robot = URDF.from_xml_string(description)
# Get left joint wheel
joint_left = get_joint(robot, self.left_wheel_name)
# Get right joint wheel
joint_right = get_joint(robot, self.right_wheel_name)
# Measure wheel separation
self.wheel_separation = euclidean_of_vectors(joint_left.origin.xyz,
joint_right.origin.xyz)
# Get radius joint
link_left = get_link(robot, joint_left.child)
self.radius = link_left.collision.geometry.radius
self.get_logger().info(
f"Wheel separation {self.wheel_separation} - Radius {self.radius}")
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, urdf_path, mu=0.7):
# parse urdf
urdf_model = URDF.from_xml_file(urdf_path)
lfoot_link_name = "LFoot"
rfoot_link_name = "RFoot"
lfoot_link = [
link for link in urdf_model.links if link.name == lfoot_link_name
][0]
rfoot_link = [
link for link in urdf_model.links if link.name == rfoot_link_name
][0]
self.foot_size = lfoot_link.collision.geometry.size
self.foot_sole_position = np.array([
lfoot_link.collision.origin.xyz[0],
lfoot_link.collision.origin.xyz[1],
lfoot_link.collision.origin.xyz[2] - self.foot_size[2] / 2.0
])
print("foot_size: ", self.foot_size)
print("foot_sole_position: ", self.foot_sole_position)
# zmp constraints
dx_min, dx_max = -self.foot_size[0] / 2.0, self.foot_size[0] / 2.0
dy_min, dy_max = -self.foot_size[1] / 2.0, self.foot_size[1] / 2.0
self.zmpConsSpatialForce = np.array([[-1, 0, 0, 0, 0, dy_min],
[1, 0, 0, 0, 0, -dy_max],
[0, 1, 0, 0, 0, dx_min],
[0, -1, 0, 0, 0, -dx_max]])
# friction constraints
self.mu = mu
self.frictionConsSpatialForce = np.array([[0, 0, 0, 1, 0, -self.mu],
[0, 0, 0, -1, 0, -self.mu],
[0, 0, 0, 0, 1, -self.mu],
[0, 0, 0, 0, -1, -self.mu]])
# unilateral constraints
self.unilateralConsSpatialForce = np.array([[0, 0, 0, 0, 0, -1]])
# GRF constraints: zmp constraints + friction constraints + unilateral constraints
self.SpatialForceCons = np.vstack(
(self.zmpConsSpatialForce, self.frictionConsSpatialForce,
self.unilateralConsSpatialForce))
def mrl_interpretor():
pub = rospy.Publisher('joint_command', JointState, queue_size=10)
nh = rospy.init_node('mrl_interpretor', anonymous=True)
rate = rospy.Rate(10) # 10hz
i = 0
msg = JointState() #生成msg对象
robot = URDF.from_parameter_server()
msg.header.frame_id = robot.get_root() #"base_link"
msg.name = get_joints()
while not rospy.is_shutdown():
msg.header.seq = i
msg.position.append(Points[i][1])
msg.position.append(Points[i][2])
msg.position.append(Points[i][3])
msg.position.append(Points[i][4])
msg.position.append(Points[i][5])
msg.velocity.append(Velosity * MotionPara[i][2])
msg.velocity.append(Velosity * MotionPara[i][2])
msg.velocity.append(Velosity * MotionPara[i][2])
msg.velocity.append(Velosity * MotionPara[i][2])
msg.velocity.append(Velosity * MotionPara[i][2])
msg.header.stamp = rospy.Time.now()
rospy.loginfo(msg)
if (Points[i][0]) != (MotionPara[i][1]):
break
pub.publish(msg)
i = i + 1
del msg.position[4]
del msg.position[3]
del msg.position[2]
del msg.position[1]
del msg.position[0]
del msg.velocity[4]
del msg.velocity[3]
del msg.velocity[2]
del msg.velocity[1]
del msg.velocity[0]
rate.sleep()
def __init__(self):
rospy.loginfo("Creating BallWatcher class")
self.print_help()
self.objDetector = Obj3DDetector()
# flags and vars
# for ball dropping position
self.kinect_calibrate_flag = False
self.running_flag = False
self.start_calc_flag = False
self.ball_marker = sawyer_mk.MarkerDrawer("/base", "ball", 500)
self.drop_point = Point()
self.drop_point_arr = []
self.pos_rec_list = np.array([])
self.drop_point_marker = sawyer_mk.MarkerDrawer(
"/base", "dropping_ball", 500)
self.ik_cont = IKController()
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, BASE, EE_FRAME)
self.limb_interface = intera_interface.Limb()
self.pos_rec = [
PointStamped() for i in range(2)
] # array 1x2 with each cell storing a PointStamped with cartesian position of the ball in the past two frames
self.old_pos_rec = [PointStamped() for i in range(2)]
self.last_tf_time = rospy.Time()
self.tf_listener = tf.TransformListener()
self.tf_bc = tf.TransformBroadcaster()
self.tf_listener.waitForTransform("base", "camera_link", rospy.Time(),
rospy.Duration(0.5))
self.T_sc_p, self.T_sc_q = self.tf_listener.lookupTransform(
"base", "camera_link", rospy.Time())
self.T_sc = tr.euler_matrix(*tr.euler_from_quaternion(self.T_sc_q))
self.T_sc[0:3, -1] = self.T_sc_p
# kbhit instance
self.kb = kbhit.KBHit()
rospy.on_shutdown(self.kb.set_normal_term)
# publishers and timers
self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb)
self.tf_timer = rospy.Timer(rospy.Duration(0.01), self.tf_update_cb)
self.final_x_total = 0
self.final_y_total = 0
def __init__(self,urdfname,ratet):
self.aubo_q=[]
# self.aubo_q=[0,pi/2,pi/2,pi/2,0,0]
self.robot = URDF.from_xml_file(urdfname)
self.detat=float(1.0/ratet)
# self.aubo5=Renovation_operation()
self.netf_reader = NetfData()
self.netf_sub = rospy.Subscriber("/robotiq_ft_wrench", WrenchStamped, self.netf_reader.callback)
# self.aubo_pose_sub = rospy.Subscriber('/renov_up_level/aubo_pose', Pose, self.obtain_aubo_pose)
self.aubo_joint_sub = rospy.Subscriber('/renov_up_level/aubo_joints', JointState, self.obtain_aubo_joints)
# self.aubo_vel_sub = rospy.Subscriber('/renov_up_level/aubo_vel', catersian_vel,queue_size=10)
# self.aubo_status_sub = rospy.Subscriber('/renov_up_level/aubo_status',physical_para ,queue_size=10)
self.aubo_move_track_pub=rospy.Publisher('/aubo_ros_script/movet', String, queue_size=1)
self.aubo_move_joint_pub = rospy.Publisher('/aubo_ros_script/movej', String, queue_size=1)
self.aubo_move_line_pub = rospy.Publisher('/aubo_ros_script/movel', String, queue_size=1)
def xacro(self, file, args=''):
"""
Generates a URDF from a XACRO file. If the URDF is invalid the test is
automatically failed.
@param file: The name of the xacro input file within `franka_description/robots/`
@param args: A optional string of xacro args to append, e.g. ("foo:=1 bar:=true")
@return: The generated URDF, as urdf_parser_py.urdf.URDF type
"""
try:
return URDF.from_xml_string(
subprocess.check_output(
'xacro $(rospack find %s)/robots/%s %s' %
(PKG, file, args),
shell=True))
except subprocess.CalledProcessError as e:
self.fail(
'Could not generate URDF from "%s", probably syntax error: %s'
% (file, e.output))
def __init__(self, urdf, default_joint_vel_limit=0):
"""
:param urdf:
:type urdf: str
:param joints_to_symbols_map: maps urdf joint names to symbols
:type joints_to_symbols_map: dict
:param default_joint_vel_limit: all velocity limits which are undefined or higher than this will be set to this
:type default_joint_vel_limit: float
"""
self.default_joint_velocity_limit = default_joint_vel_limit
self.default_weight = 0.0001
self.fks = {}
self._joint_to_frame = {}
self.joint_to_symbol_map = keydefaultdict(lambda x: spw.Symbol(x))
self.urdf = urdf
with suppress_stderr():
self._urdf_robot = URDF.from_xml_string(
hacky_urdf_parser_fix(self.urdf))
def nizhuan(q):
RR = array([[0,1,0],[1,0,0],[0,0,-1]])
n= mat([[0.0,1.0,0.0],[1.0,0.0,0.0],[0.0,0.0,-1.0]])
print(n)
robot = URDF.from_xml_file("/home/xsm/control/src/learning_communication/src/ur5_hole.urdf")
tree = kdl_tree_from_urdf_model(robot)
kdl_kin = KDLKinematics(robot, "base_link", "ee_link", tree)
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
pose[2,1] = -pose[2,1]
pose[2,2] = -pose[2,2]
pose[0,0] = -pose[0,0]
pose[1,0] = -pose[1,0]
n[0:3,0] = pose[0:3,1]
n[0:3,1] = pose[0:3,2]
n[0:3,2] = pose[0:3,0]
n = linalg.pinv(dot(RR,n))
return array(n)
def __init__(self):
self.urdf=URDF.from_parameter_server()
self.payloads=dict()
self.payload_targets=dict()
self.link_markers=dict()
self.payloads_lock=threading.Lock()
self._ros_id=1
#self._pub_aco_listener = PayloadManagerSubscriberListener(self)
#self._pub_aco=rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100, subscriber_listener = self._pub_aco_listener)
self._pub_planning_scene=rospy.Publisher("planning_scene", PlanningScene, latch = True, queue_size=10)
self._payload_msg_pub=rospy.Publisher("payload", PayloadArray, queue_size=100)
self.rviz_cam_publisher=rospy.Publisher("payload_marker_array", MarkerArray, queue_size=100, latch=True)
self._payload_msg_sub=rospy.Subscriber("payload", PayloadArray, self._payload_msg_cb)
self._update_payload_pose_srv=rospy.Service("update_payload_pose", UpdatePayloadPose, self._update_payload_pose_srv_cb)
self._get_payload_array_srv=rospy.Service("get_payload_array", GetPayloadArray, self._get_payload_array_srv_cb)
if _use_tesseract:
self._tesseract_pub = rospy.Publisher("tesseract_diff", TesseractState, latch=True, queue_size=10)
def _get_joint_info(joint_name):
limit = None
mimics = {}
robot = URDF.from_parameter_server(key='/robot_description')
for joint in robot.joints:
if joint.name == joint_name:
limit = joint.limit
elif joint.mimic is not None:
if joint.mimic.joint == joint_name:
mimics[joint.name] = joint.mimic
if limit is None:
raise RuntimeError(
'Cannot find limits for joint "{}" in the robot description'.
format(joint_name))
return limit, mimics
def touch_and_refine_table(robot, scene, move_group):
urdf_robot = URDF.from_parameter_server()
kdl_kin = KDLKinematics(urdf_robot, urdf_robot.links[1].name,
urdf_robot.links[8].name)
move_group.set_max_velocity_scaling_factor(
0.1) # Allow 10 % of the set maximum joint velocities
move_group.set_max_acceleration_scaling_factor(0.05)
# Receive table message
go_home()
print("Waiting for table message ...")
the_table = rospy.wait_for_message("/octomap_new/table", table)
touchPoses = []
for i in range(10):
pose_goal = go_to_random_point_over_table(move_group, the_table)
touchSensorMsg = touch_table_straight_on(move_group, robot, kdl_kin,
pose_goal)
print("Touched the surface")
# Check if usable collision
if '/panda/bumper/panda_probe_ball' in touchSensorMsg.collidingLinks:
rospy.sleep(2.)
ballSensorMsg = ContactsState()
while ballSensorMsg.states == []:
ballSensorMsg = rospy.wait_for_message(
"/panda/bumper/panda_probe_ball", ContactsState)
#print(ballSensorMsg)
touchPoses.append(ballSensorMsg)
else:
print("Collided with wrong part; ignored")
i -= 1
print("Recording done, retracting ...")
retract_from_table_straight(move_group, robot, kdl_kin)
# note eef position when collided, e.g. by listening to /panda/panda/colliding; in real probably ask libfranka
fit_table(touchPoses)
def get_cuboid_config():
joint_angles = []
# assert len(joint_angles) == 5, "Incorrect number of joints specified."
joint_table, axis_table = {}, {}
robot = URDF.from_xml_file(F_path)
joint_names = robot.get_chain('arm_base_link', 'gripper_link', links=False)
for i in joint_names:
joint_info = robot.joint_map[i]
joint_table[i] = joint_info.origin
axis_table[i] = joint_info.axis
wrist_pose = getWristPose(joint_angles, joint_names, axis_table,
joint_table)
jacobian = getWristJacobian(joint_angles, joint_names, joint_table,
axis_table, wrist_pose)
print("Wrist pose: {}".format(
np.array_str(np.array(wrist_pose), precision=3)))
print("Jacobian: {}".format(np.array_str(np.array(jacobian), precision=3)))
