Skip to content

Instantly share code, notes, and snippets.

@708yamaguchi
Created November 22, 2021 11:19
Show Gist options
  • Save 708yamaguchi/2c754cbfc1425345aceb5c3e7a66ae22 to your computer and use it in GitHub Desktop.
Save 708yamaguchi/2c754cbfc1425345aceb5c3e7a66ae22 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_controller/follow_joint_trajectory/goal",
FollowJointTrajectoryActionGoal,
queue_size=1)
def move_fetch(pos, time):
now = rospy.Time.now()
goal = FollowJointTrajectoryActionGoal()
goal.header.stamp = now
goal.goal.trajectory.header.stamp = now
joint_names = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint']
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 fetch init pose before calling these lines
rospy.sleep(1)
move_fetch(0.0, 10) # init pose
rospy.sleep(1)
move_fetch(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