import math
import numpy as np
from franka_robot import FrankaRobot
if __name__ == '__main__':
dh_params = np.array([[0, 0.333, 0, 0], [0, 0, -math.pi / 2, 0],
[0, 0.316, math.pi / 2, 0],
[0.0825, 0, math.pi / 2, 0],
[-0.0825, 0.384, -math.pi / 2, 0],
[0, 0, math.pi / 2, 0], [0.088, 0, math.pi / 2, 0],
[0, 0.107, 0, 0], [0, 0.1034, 0, 0]])
fr = FrankaRobot('franka_robot.urdf', dh_params, 7)
joints = [
0, -math.pi / 4, 0, -3 * math.pi / 4, 0, math.pi / 2, math.pi / 4
]
ee = fr.ee(joints)
ee[1] += 0.01
new_joints = fr.inverse_kinematics(ee, joints)
new_ee = fr.ee(new_joints)
print(np.allclose(new_ee, ee))
class RobotStatePublisher():
def __init__(self):
self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
self.marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
self.box_sub = rospy.Subscriber('/basic_controls/feedback', InteractiveMarkerFeedback, self.box_callback)
rospy.init_node('joint_state_publisher')
self.hz = 10
self.rate = rospy.Rate(self.hz) # 30 hz
self.joint_state = JointState()
self.joint_state.name = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2']
self.joint_state.position = [0.0, -math.pi /4, 0.0, -3 * math.pi / 4, 0.0, math.pi / 2, math.pi / 4, 0.0, 0.0]
self.red_cylinder = Marker()
self.red_cylinder.header.frame_id = "panda_link0"
self.red_cylinder.type = Marker.CYLINDER
self.red_cylinder.pose.position.x = 0
self.red_cylinder.pose.position.y = 0.3
self.red_cylinder.pose.position.z = 0
self.red_cylinder.color.a = 1.0
self.red_cylinder.color.r = 1.0
self.red_cylinder.color.g = 0.0
self.red_cylinder.color.b = 0.0
self.red_cylinder.scale.x = 0.1
self.red_cylinder.scale.y = 0.1
self.red_cylinder.scale.z = 0.1
self.green_cylinder = Marker()
self.green_cylinder.header.frame_id = "panda_link0"
self.green_cylinder.type = Marker.CYLINDER
self.green_cylinder.pose.position.x = 0
self.green_cylinder.pose.position.y = 0.3
self.green_cylinder.pose.position.z = 0
self.green_cylinder.color.a = 1.0
self.green_cylinder.color.r = 0.0
self.green_cylinder.color.g = 1.0
self.green_cylinder.color.b = 0.0
self.green_cylinder.scale.x = 0.1
self.green_cylinder.scale.y = 0.1
self.green_cylinder.scale.z = 0.1
self.dh_params = np.array([[0, 0.333, 0, 0],
[0, 0, -math.pi/2, 0],
[0, 0.316, math.pi/2, 0],
[0.0825, 0, math.pi/2, 0],
[-0.0825, 0.384, -math.pi/2, 0],
[0, 0, math.pi/2, 0],
[0.088, 0, math.pi/2, 0],
[0, 0.107, 0, 0],
[0, 0.1034, 0, 0]])
self.fr = FrankaRobot('franka_robot.urdf', self.dh_params, 7)
self.box = [0.5, 0, 0.21, 0, 0, 0, 0.15, 0.09, 0.126]
self.ee_trajectory = np.empty((0,6))
self.joint_trajectory = np.empty((0,7))
self.joint_trajectory_index = 0
self.num_joint_trajectory_points = 0
self.calculate_joint_trajectory()
def calculate_joint_trajectory(self):
current_joints = self.joint_state.position[:7]
current_ee_pose = self.fr.ee(current_joints)
desired_goal_pose_1 = np.copy(current_ee_pose)
desired_goal_pose_1[0] += 0.1
desired_goal_pose_1[1] += 0.1
desired_goal_pose_2 = np.copy(current_ee_pose)
desired_goal_pose_2[2] += 0.1
desired_goal_pose_2[5] += math.pi/2
ee_trajectory_1 = lin_interp(current_ee_pose,desired_goal_pose_1,3*self.hz)
ee_trajectory_2 = lin_interp(desired_goal_pose_1,desired_goal_pose_2,3*self.hz)
ee_trajectory_3 = lin_interp(desired_goal_pose_2,current_ee_pose,3*self.hz)
self.ee_trajectory = np.concatenate([ee_trajectory_1,ee_trajectory_2,ee_trajectory_3], axis=0)
self.num_joint_trajectory_points = self.ee_trajectory.shape[0]
self.joint_trajectory = np.zeros((self.num_joint_trajectory_points,7))
for i in range(self.num_joint_trajectory_points):
print(i)
self.joint_trajectory[i,:] = self.fr.inverse_kinematics(self.ee_trajectory[i,:], current_joints)
current_joints = self.joint_trajectory[i,:]
def box_callback(self, data):
self.box[0] = data.pose.position.x
self.box[1] = data.pose.position.y
self.box[2] = data.pose.position.z
euler = tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
self.box[3] = euler[0]
self.box[4] = euler[1]
self.box[5] = euler[2]
def run(self):
self.joint_state.position[:7] = self.joint_trajectory[self.joint_trajectory_index,:]
self.joint_trajectory_index = (self.joint_trajectory_index + 1) % self.num_joint_trajectory_points
if(self.fr.check_box_collision(self.joint_state.position, self.box)):
self.red_cylinder.header.stamp = rospy.Time.now()
self.marker_pub.publish(self.red_cylinder)
else:
self.green_cylinder.header.stamp = rospy.Time.now()
self.marker_pub.publish(self.green_cylinder)
self.joint_state.header.stamp = rospy.Time.now()
self.joint_state_pub.publish(self.joint_state)
self.rate.sleep()