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)
