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, 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 __init__(self, limb, hover_distance=0.15, verbose=True):
self._baxter = URDF.from_parameter_server(
key='robot_description'
) #Get Baxter URDF Model from ROS Parameter Server
self._kdl_tree = kdl_tree_from_urdf_model(
self._baxter) #Get kdl tree Baxter URDF Model
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb) #Creates a Limb Object
self._gripper = baxter_interface.Gripper(
limb) #Creates a Gripper Object from the Limb Object
#self._kin = baxter_kinematics(limb)
#self._traj = Trajectory(self._limb_name)
self._base_link = self._baxter.get_root(
) #Gets root link of the Baxter URDF model
self._tip_link = limb + '_gripper' #Gets the gripper link of ther respective limb
self._arm_chain = self._kdl_tree.getChain(
self._base_link,
self._tip_link) #Creates chain from root link and gripper link
self._jac_kdl = PyKDL.ChainJntToJacSolver(
self._arm_chain
) #Creates a jacobian solver for the Baxter from the chain
self._joint_names = self._limb.joint_names(
) #List of joint names of the respective limb
self._trajectory = list() #List of Nominal Trajectory Waypoints
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
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 = 'right_hand'
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self.jac_solver = PyKDL.ChainJntToJacSolver(self._arm_chain)
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
def main():
# ipdb.set_trace()
data = dmp_ik_test.main()
train_len = len(data)
end_frame = PyKDL.Frame()
end_frame_dmp = PyKDL.Frame()
baxter = URDF.from_parameter_server(key='robot_description')
base_link = baxter.get_root()
limb = "left"
tip_link = limb + '_gripper'
kdl_tree = kdl_tree_from_urdf_model(baxter)
arm_chain = kdl_tree.getChain(base_link, tip_link)
fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(arm_chain)
Jnt_list = PyKDL.JntArray(7)
Jnt_list_dmp = PyKDL.JntArray(7)
pose = []
pose_dmp = []
for i in range(train_len):
Jnt_list[0] = data[0][i]
Jnt_list[1] = data[1][i]
Jnt_list[2] = data[2][i]
Jnt_list[3] = data[3][i]
Jnt_list[4] = data[4][i]
Jnt_list[5] = data[5][i]
Jnt_list[6] = data[6][i]
fk_p_kdl.JntToCart(Jnt_list, end_frame)
pos = end_frame.p
rot = PyKDL.Rotation(end_frame.M)
rot = rot.GetQuaternion()
pose.append(
np.array([pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]]))
xs = []
ys = []
zs = []
for j in range(train_len):
position_x = pose[j][0]
position_y = pose[j][1]
position_z = pose[j][2]
xs.append(position_x)
ys.append(position_y)
zs.append(position_z)
fig = plt.figure(2)
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
# plot traj fig
ax.plot(xs, ys, zs, linewidth=4, alpha=0.3)
# Plot plan fig
# Plot plan fig
# show the plot
plt.draw()
plt.show()
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, rospy_init=False):
if rospy_init:
rospy.init_node('baxter_kinematics')
# KDL chain from URDF
self._urdf = URDF.from_parameter_server(key='robot_description')
self._base_link = 'left_arm_mount' #self._urdf.get_root()
self._tip_link = 'left_wrist'
self._kdl_tree = kdl_tree_from_urdf_model(self._urdf)
self._kdl_chain = self._kdl_tree.getChain(self._base_link, self._tip_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._kdl_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._kdl_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._kdl_chain, self._fk_p_kdl, self._ik_v_kdl)
# Baxter Interface Limb Instances
self._limb = baxter_interface.Limb('left')
self._joint_names = self._limb.joint_names()
self._joint_names = [jn for jn in self._joint_names] # left_e0 is fixed
self._num_jnts = len(self._joint_names)
def __init__(self, limb):
self._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())
joint0_data_dmp = train_set_dmp[:, 0]
joint1_data_dmp = train_set_dmp[:, 1]
joint2_data_dmp = train_set_dmp[:, 2]
joint3_data_dmp = train_set_dmp[:, 3]
joint4_data_dmp = train_set_dmp[:, 4]
joint5_data_dmp = train_set_dmp[:, 5]
joint6_data_dmp = train_set_dmp[:, 6]
end_frame = PyKDL.Frame()
end_frame_dmp = PyKDL.Frame()
baxter = URDF.from_parameter_server(key='robot_description')
base_link = baxter.get_root()
limb = "right"
tip_link = limb + '_gripper'
kdl_tree = kdl_tree_from_urdf_model(baxter)
arm_chain = kdl_tree.getChain(base_link, tip_link)
fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(arm_chain)
Jnt_list = PyKDL.JntArray(7)
Jnt_list_dmp = PyKDL.JntArray(7)
pose = []
pose_dmp = []
for i in range(train_len):
Jnt_list[0] = joint0_data[i]
Jnt_list[1] = joint1_data[i]
Jnt_list[2] = joint2_data[i]
Jnt_list[3] = joint3_data[i]
Jnt_list[4] = joint4_data[i]
Jnt_list[5] = joint5_data[i]
Jnt_list[6] = joint6_data[i]
fk_p_kdl.JntToCart(Jnt_list, end_frame)