Control denso robot by inputting joint angle

edit

Hi all. When I'm trying to control denso robot VS-068 with denso-robot-vs060 package, I want to input arm joint angle data from the json file I prepared before. ((Of course, I renamed robot name in the urdf files.)

Which denso-package should I input the angle data? "bcap-core" package?

The python program for input angle data to rviz is shown below. (In this program, I input angle data into "denso_launch" and "pr2_controllers_msgs")

Using this file, I can control VS-068 on RViz "denso_vs060_moveit_demo_simulation.launch"

#!/usr/bin/env python
import sys
import math
import json

import roslib 
roslib.load_manifest('denso_launch')
#roslib.load_manifest('denso_robot_bringup')
roslib.load_manifest('pr2_controllers_msgs')

import rospy, actionlib
from pr2_controllers_msgs.msg import *

def get_angle_list(filename):
    file = open(filename, 'r')

    json_dict = json.load(file)

    angle_list = json_dict["angle_list"]
    time_list =  json_dict["time_list"]
    file.close()

    return angle_list, time_list

if __name__ == '__main__':
    args = sys.argv

    try:
        rospy.init_node('send_angles', anonymous=True)
        client = actionlib.SimpleActionClient('/arm_controller/joint_trajectory_action', JointTrajectoryAction)
        client.wait_for_server()

        goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
        goal.trajectory.joint_names.append("j1")
        goal.trajectory.joint_names.append("j2")
        goal.trajectory.joint_names.append("j3")
        goal.trajectory.joint_names.append("j4")
        goal.trajectory.joint_names.append("j5")
        goal.trajectory.joint_names.append("flange")
        print goal.trajectory.joint_names


        positions, times =  get_angle_list(args[1])
        positions.insert(0, [0.0, 0.0, math.pi/2,  0.0, 0.0, 0.0] )
        times.insert(0, -0.1)

        goal.trajectory.points = []

        for i in range(len(positions)):
            point = trajectory_msgs.msg.JointTrajectoryPoint()
            point.positions = positions[i]
            goal.trajectory.points.append(point)
            goal.trajectory.points[i].time_from_start = rospy.Duration(times[i] + 1.0)

        goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(1.0)

        client.send_goal(goal)

        print client.wait_for_result()
    except rospy.ROSInterruptException: pass