def process(msg): global curr_angle, count, last g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES # given the end-effector position, get joint angles ur5_kin = ikfastpy.PyKinematics() n_joints = ur5_kin.getDOF() # theta = np.linspace(0.1, 2 * np.pi, 13) # x = [] # y = [] # for i in range(len(theta)): # x.append(np.sin(theta[i])) # y.append(np.cos(theta[i])) # last = np.array([ 1.14939153,-2.84839082,-0.97130454,-1.93128979,-1.18349659, 2.71537089],dtype='float32') # global t # t = (t + 1)%13 # ee_pose = np.array([[ 0.56230879,0.48327458,0.67101002,x[t]* 0.6 + 0.1], # [-0.80670726,0.14224425,0.57357645,y[t] *0.6 + 0.1], # [ 0.18174762,-0.86383575,0.46984631, 0.6]]) ee_pose = np.array(msg.data).reshape(3, 4) print("post", ee_pose) joint_configs = ur5_kin.inverse(ee_pose.reshape(-1).tolist()) n_solutions = int(len(joint_configs) / n_joints) print(t, ee_pose[0][3], ee_pose[1][3], ee_pose[2][3]) print(" %d solutions found:" % n_solutions) joint_configs = np.asarray(joint_configs).reshape(n_solutions, n_joints) if (len(joint_configs) == 0): joint_angles = last # print("not found") else: dis = [] for i in range(len(joint_configs)): dis.append(np.linalg.norm(last - joint_configs[i], 2)) min_dis_index = dis.index(min(dis)) joint_angles = joint_configs[min_dis_index] last = joint_angles # publish goal print("angles", joint_angles) g.trajectory.points = [ JointTrajectoryPoint(positions=joint_angles, velocities=[0] * 6, time_from_start=rospy.Duration(1.0)) ] client.send_goal(g) try: client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise
def move(): global curr_angle, count, last g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES # given the end-effector position, get joint angles ur5_kin = ikfastpy.PyKinematics() n_joints = ur5_kin.getDOF() global x, y, ee_post_left, ee_post_right, diff, t t = (t + 1) % len(x) ee_pose = ee_post_left ee_pose[0][3] = x[t] ee_pose[1][3] = y[t] print("pose", ee_pose) joint_configs = ur5_kin.inverse(ee_pose.reshape(-1).tolist()) n_solutions = int(len(joint_configs) / n_joints) print(t, ee_pose[0][3], ee_pose[1][3], ee_pose[2][3]) print(" %d solutions found:" % n_solutions) joint_configs = np.asarray(joint_configs).reshape(n_solutions, n_joints) if (len(joint_configs) == 0): joint_angles = last # print("not found") else: dis = [] for i in range(len(joint_configs)): dis.append(np.linalg.norm(last - joint_configs[i], 2)) min_dis_index = dis.index(min(dis)) joint_angles = joint_configs[min_dis_index] last = joint_angles # publish goal print("angles", joint_angles) g.trajectory.points = [ JointTrajectoryPoint(positions=joint_angles, velocities=[0] * 6, time_from_start=rospy.Duration(1.0)) ] client.send_goal(g) try: client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise
def __init__(self, MjSim, MjViewer=None, Render=False): armActuatorNames = [ 'shoulder_pan_T', 'shoulder_lift_T', 'forearm_T', 'wrist_1_T', 'wrist_2_T', 'wrist_3_T' ] gripperActuatorNames = ['gripper'] armJointNames = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] gripperJointNames = ['l_f', 'r_f'] cameraNames = ['c1'] self.sim = MjSim self.viewer = MjViewer self.RENDER = Render self.sim_init_state = MjSim.get_state( ) # get sim initial state, later used for reset sim # extracting qpos id for joints self.armJointQposID = [] for name in armJointNames: self.armJointQposID.append(MjSim.model._joint_name2id[name]) self.armJointAngles = np.array([0., 0., 0., 0., 0., 0.]) # in radians self.ur5_kin = ikfastpy.PyKinematics() # ur5 kinematics class self.numArmJoints = self.ur5_kin.getDOF() self.ee_pose = np.asarray(self.ur5_kin.forward( self.armJointAngles)).reshape(3, 4) # 3x4 rigid transformation matrix # extracting qpos id for gripper self.gripperJointQposID = [] for name in gripperJointNames: self.gripperJointQposID.append(MjSim.model._joint_name2id[name]) self.gripperJointAngles = np.array([0., 0.]) # extracting ctrl id for arm joints self.armCtrlID = [] for name in armActuatorNames: self.armCtrlID.append(MjSim.model._actuator_name2id[name]) # extracting ctrl id for gripper joints self.gripperCtrlID = MjSim.model._actuator_name2id[ gripperActuatorNames[0]] self.currentToolPosition = None self.currentToolQuaternion = None
def __init__(self): self.ur5_kin = ikfastpy.PyKinematics() self.n_joints = self.ur5_kin.getDOF() rospy.init_node('controller_command', anonymous=True) self.hz = 100 self.rate = rospy.Rate(self.hz) self.pub = rospy.Publisher('scaled_pos_joint_traj_controller/command', JointTrajectory, queue_size=10) self.sub = rospy.Subscriber('scaled_pos_joint_traj_controller/state', JointTrajectoryControllerState, self.get_current_state_cb) self.sleep(0.3) self.current_state = None self.joint_names = None while self.current_state is None: pass print("controller_command init finished")
def __init__(self, MjSim, MjViewer=None, Render=False): # When initializing, extract qpos id and ctrl id # qpos id for check joint values and velocities # ctrl id for assigning commands to position/velocity actuators armPositionActuatorNames = [ 'shoulder_pan', 'shoulder_lift', 'forearm', 'wrist_1', 'wrist_2', 'wrist_3' ] # arm position actuators armVelocityActuatorNames = [ 'shoulder_pan_v', 'shoulder_lift_v', 'forearm_v', 'wrist_1_v', 'wrist_2_v', 'wrist_3_v' ] # arm velocity actuators gripperPositionActuatorNames = ['gripper' ] # gripper position actuators gripperVelocityActuatorNames = ['gripper_v' ] # gripper velocity actuators armJointNames = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] gripperJointNames = ['l_f', 'r_f'] cameraNames = ['c1'] gripperMainTouchNames = [ 'lf_main', 'rf_main' ] # main touch sensors are used to check whether or not gripper is fully closed self.sim = MjSim self.viewer = MjViewer self.RENDER = Render # bool to determine whether or not visualize simulation self.sim_init_state = MjSim.get_state( ) # get sim initial state, later used for reset sim # extracting qpos id for joints self.armJointQposID = [] for name in armJointNames: self.armJointQposID.append(MjSim.model._joint_name2id[name]) self.armJointAngles = np.array([0., 0., 0., 0., 0., 0.]) # in radians # extracting qpos id for gripper self.gripperJointQposID = [] for name in gripperJointNames: self.gripperJointQposID.append(MjSim.model._joint_name2id[name]) self.gripperJointAngles = np.array([0., 0.]) # extracting ctrl id for arm position actuators self.armPositionCtrlID = [] for name in armPositionActuatorNames: self.armPositionCtrlID.append(MjSim.model._actuator_name2id[name]) # extracting ctrl id for arm velocity actuators self.armVelocityCtrlID = [] for name in armVelocityActuatorNames: self.armVelocityCtrlID.append(MjSim.model._actuator_name2id[name]) # extracting ctrl id for gripper actuator self.gripperPositionCtrlID = MjSim.model._actuator_name2id[ gripperPositionActuatorNames[0]] self.gripperVelocityCtrlID = MjSim.model._actuator_name2id[ gripperVelocityActuatorNames[0]] # extracting gripper main touch sensor id self.gripperMainTouchID = [] for name in gripperMainTouchNames: self.gripperMainTouchID.append(MjSim.model._sensor_name2id[name]) self.ur5_kin = ikfastpy.PyKinematics() # ur5 kinematics class self.numArmJoints = self.ur5_kin.getDOF() self.ee_pose = np.asarray(self.ur5_kin.forward( self.armJointAngles)).reshape(3, 4) # 3x4 rigid transformation matrix self.currentToolPosition = None self.currentToolQuaternion = None self.timeStep = MjSim.model.opt.timestep # simulation timestep
import numpy as np import ikfastpy import datetime t1 = datetime.datetime.now() # Initialize kinematics for UR5 robot arm ur5_kin = ikfastpy.PyKinematics() n_joints = ur5_kin.getDOF() # joint_angles = [-3.1,-1.6,1.6,-1.6,-1.6,0.] # in radians joint_angles = [0.0, 0.3, 0.15, 0.0, 0.4, 0.0] # in radians # joint_angles = [0.0016144849504478032, 0.02851116174066437, 0.013754926474105922, -0.012710444727898285, 0.003797917117521088, -0.00937656560536304] # in radians # joint_angles = [0.,0.,0.,0.,0.,0.] # in radians def select(q_sols, q_d, w=[1] * 6): """ Select the optimal solutions among a set of feasible joint value solutions. Args: q_sols: A set of feasible joint value solutions (unit: radian) q_d: A list of desired joint value solution (unit: radian) w: A list of weight corresponding to robot joints Returns: A list of optimal joint value solution. """ error = [] for q in q_sols: error.append(sum([w[i] * (q[i] - q_d[i])**2 for i in range(6)]))
import ikfastpy right_ik = ikfastpy.PyKinematics()
def __init__(self, bullet_client, offset): if useIKFast: # Initialize kinematics for UR5 robot arm self.xarm_kin = ikfastpy.PyKinematics() self.n_joints = self.xarm_kin.getDOF() print("numJoints IKFast=", self.n_joints) self.bullet_client = bullet_client self.offset = np.array(offset) self.jointPoses = [0] * xarmNumDofs #print("offset=",offset) flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES legos = [] self.bullet_client.loadURDF("tray/traybox.urdf", [0.4 + offset[0], offset[1], offset[2]], [0, 0, 0, 1], flags=flags) legos.append( self.bullet_client.loadURDF("lego/lego.urdf", np.array([0.3, 0.1, 0.3]) + self.offset, flags=flags)) legos.append( self.bullet_client.loadURDF("lego/lego.urdf", np.array([0.3, -0.1, 0.3]) + self.offset, flags=flags)) legos.append( self.bullet_client.loadURDF("lego/lego.urdf", np.array([0.5, 0.1, 0.3]) + self.offset, flags=flags)) sphereId = self.bullet_client.loadURDF("sphere_small.urdf", np.array([0.4, 0, 0.3]) + self.offset, flags=flags) self.bullet_client.loadURDF("sphere_small.urdf", np.array([0.3, 0, 0.3]) + self.offset, flags=flags) self.bullet_client.loadURDF("sphere_small.urdf", np.array([0.5, 0, 0.3]) + self.offset, flags=flags) orn = [0, 0, 0, 1] self.xarm = self.bullet_client.loadURDF("xarm/xarm6_robot.urdf", np.array([0, 0, 0]) + self.offset, orn, useFixedBase=True, flags=flags) index = 0 for j in range(self.bullet_client.getNumJoints(self.xarm)): self.bullet_client.changeDynamics(self.xarm, j, linearDamping=0, angularDamping=0) info = self.bullet_client.getJointInfo(self.xarm, j) jointName = info[1] jointType = info[2] if (jointType == self.bullet_client.JOINT_PRISMATIC): self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) index = index + 1 if (jointType == self.bullet_client.JOINT_REVOLUTE): self.bullet_client.resetJointState(self.xarm, j, jointPositions[index]) index = index + 1 self.t = -math.pi / 4.0
import ikfastpy left_ik = ikfastpy.PyKinematics()
def atsCallback(self, cur_state, target): print "listen to joint state" msg = cur_state state = {} #get current state for i, name in enumerate(msg.name): if name in self.JOINT_NAMES: # print(i, name, msg.position[i]) state[name] = msg.position[i] for i in range(len(self.JOINT_NAMES)): self.last[i] = state.get(self.JOINT_NAMES[i]) print(self.last) # initial ik solver g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() # g.trajectory.header = Header() # g.trajectory.header.stamp = rospy.Time.now() g.trajectory.joint_names = self.JOINT_NAMES # given the end-effector position, get joint angles ur5_kin = ikfastpy.PyKinematics() n_joints = ur5_kin.getDOF() prev_ee_pose = ur5_kin.forward(self.last) prev_ee_pose = np.asarray(prev_ee_pose).reshape(3, 4) ee_pose = np.array(target.data).reshape(3, 4) # get distance between current state to target, velocity diff = prev_ee_pose - ee_pose dist = np.linalg.norm(diff[:, -1]) dist_max = 1 / 2.0 * 1 * self.time_step**2 print(ee_pose) print("dist", dist, "dist max", dist_max) joint_configs = ur5_kin.inverse(ee_pose.reshape(-1).tolist()) n_solutions = int(len(joint_configs) / n_joints) # print(t, ee_pose[0][3], ee_pose[1][3], ee_pose[2][3]) # print(" %d solutions found:"%n_solutions) joint_configs = np.asarray(joint_configs).reshape( n_solutions, n_joints) print("diff") print(diff) if (len(joint_configs) == 0): joint_angles = self.last # print("not found") # if (dist < dist_max): # diff_list = else: for i in range(10): # divided a target into 10 small target if (dist < dist_max): diff_i = diff[0, 3] * i / 10 print('1') else: # d = 1/2 * a * (t+dt)^2 - 1/2 * a * t^2 = a * dt + a/2 # d1 = ((i + 1) * self.time_step / 10)**2 # d2 = (i * self.time_step / 10)**2 # diff_i = self.max_acc * (d1-d2)/2.0 # diff_i = dist_max/20.0 * i diff_i = diff[0, 3] * i / 10 # print('2', self.max_acc, self.time_step, diff_i, d1, d2) # print("diff_i ", diff_i) ee_pose_i = ee_pose ee_pose_i[1, 3] = prev_ee_pose[0, 3] + diff_i # print("ee_pose_i",ee_pose_i) # prev_ee_pose = ee_pose_i joint_configs = ur5_kin.inverse(ee_pose_i.reshape(-1).tolist()) # n_solutions = int(len(joint_configs)/n_joints) # joint_configs = np.asarray(joint_configs).reshape(n_solutions,n_joints) if (len(joint_configs) == 0): joint_angles = self.last continue else: n_solutions = int(len(joint_configs) / n_joints) joint_configs = np.asarray(joint_configs).reshape( n_solutions, n_joints) dis = [] for j in range(len(joint_configs)): dis.append( np.linalg.norm(self.last - joint_configs[j], 2)) min_dis_index = dis.index(min(dis)) joint_angles = joint_configs[min_dis_index] self.last = joint_angles # publish goal # print ("angles",joint_angles) # print("i ", i) # g.trajectory.points = [ # JointTrajectoryPoint(positions=joint_angles, velocities=[0]*6, time_from_start=rospy.Duration(1.0))] # g.trajectory.joint_names.append(self.JOINT_NAMES) point = JointTrajectoryPoint( positions=joint_angles, velocities=[0] * 6, time_from_start=rospy.Duration(i * 0.05 + 1.0)) g.trajectory.points.append(point) # else: # dis = [] # for i in range(len(joint_configs)): # dis.append(np.linalg.norm(self.last - joint_configs[i],2)) # min_dis_index = dis.index(min(dis)) # joint_angles = joint_configs[min_dis_index] # self.last = joint_angles # # publish goal # print ("angles",joint_angles) # g.trajectory.points = [ # JointTrajectoryPoint(positions=joint_angles, velocities=[0]*6, time_from_start=rospy.Duration(1.0))] print("g", g) self.client.send_goal(g)