Skip to content

Instantly share code, notes, and snippets.

@708yamaguchi
Created November 22, 2021 13:34
Show Gist options
  • Save 708yamaguchi/a2a982e9e9d44d356c7522376168ec6c to your computer and use it in GitHub Desktop.
Save 708yamaguchi/a2a982e9e9d44d356c7522376168ec6c to your computer and use it in GitHub Desktop.
#!/usr/bin/env python
import rospy
from control_msgs.msg import FollowJointTrajectoryActionGoal
from trajectory_msgs.msg import JointTrajectoryPoint
rospy.init_node('dummy_fjt_client')
pub = rospy.Publisher(
"/arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/goal",
FollowJointTrajectoryActionGoal,
queue_size=1)
def move_kinova(pos, time):
now = rospy.Time.now()
goal = FollowJointTrajectoryActionGoal()
goal.header.stamp = now
goal.goal.trajectory.header.stamp = now
joint_names = ['kinova_joint_1',
'kinova_joint_2',
'kinova_joint_3',
'kinova_joint_4',
'kinova_joint_5',
'kinova_joint_6']
goal.goal.trajectory.joint_names = joint_names
point = JointTrajectoryPoint()
point.positions = [pos] * len(joint_names)
point.velocities = [0] * len(joint_names)
point.accelerations = [0] * len(joint_names)
point.time_from_start = rospy.Time.from_sec(time)
goal.goal.trajectory.points = [point]
pub.publish(goal)
# Do not forget to set pr2 init pose before calling these lines
rospy.sleep(1)
move_kinova(0.0, 0.1) # init pose
# rospy.sleep(1)
# move_kinova(0.1, 10) # move little from init pose
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment