def send_to_neutral():
"""
DON'T USE THIS ON REAL ROBOT!!!
"""
temp_pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True)
# Create JointCommand message to publish commands
pubmsg = JointCommand()
pubmsg.names = names # names of joints (has to be 7)
pubmsg.position = neutral_pose # JointCommand msg has other fields (velocities, efforts) for
# when controlling in other control mode
pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE)
curr_val = deepcopy(vals)
while all(abs(neutral_pose[i]-curr_val[i]) > 0.01 for i in range(len(curr_val))):
temp_pub.publish(pubmsg)
curr_val = deepcopy(vals)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
count = 0
# Create JointCommand message to publish commands
pubmsg = JointCommand()
pubmsg.names = names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts))
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j in range(len(vals)):
if j == 4:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
pubmsg.position = vals # JointCommand msg has other fields (velocities, efforts) for
# when controlling in other control mode
# pubmsg.effort = [0.,0.,0.,0.,0.,0.,0.]
pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE)
pub.publish(pubmsg)
rate.sleep()
import rospy
from franka_core_msgs.msg import JointCommand
if __name__ == "__main__":
rospy.init_node("force_neutral_pose_startup")
pub = rospy.Publisher(
'panda_simulator/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=10)
command_msg = JointCommand()
command_msg.names = ["panda_joint%d" % (idx) for idx in range(1, 8)]
command_msg.position = [0.000, -0.785, 0.0, -2.356, 0.0, 1.57, 0.785]
command_msg.mode = JointCommand.POSITION_MODE
rospy.sleep(0.5)
start = rospy.Time.now().to_sec()
rospy.loginfo("Attempting to force robot to neutral pose...")
rospy.sleep(0.5)
while not rospy.is_shutdown() and (rospy.Time.now().to_sec() - start < 1.):
# print rospy.Time.now()
command_msg.header.stamp = rospy.Time.now()
pub.publish(command_msg)
rospy.loginfo("Robot forced to neutral pose. Complete!")
