JP2012081541A - Control device, robot, control method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a control device for appropriately changing the shape of a control object by operation of a user to a surface of the control object having a plurality of joints.SOLUTION: In this control device 101, a rotation angle acquisition unit 102 acquires a rotation angle q of a joint of a rod-like body possessed by an operation object. A contact angle acquisition unit 103 acquires a contact angle s from the output of a contact position sensor installed around the rod-like body. A sum calculation unit 104 calculates the sum (q+s) of the rotation angle q and the contact angle s. An output unit 105 outputs a control signal for increasing/decreasing a flexion angle or swing-up angle of the joint of the rod-like body in accordance with which angle range the sum (q+s) belongs to.

Description

  The present invention provides a control device, a robot, a control method, and an input device that are suitable for appropriately changing the shape of a control target by a user operation on the surface of the control target having a plurality of joints. Related to the program.

  2. Description of the Related Art Conventionally, an input technique for selecting a position or performing various instructions using a touch detection type sensor using a touch panel or the like has been proposed.

  For example, Patent Document 1 listed below proposes an input device that allows a user to intuitively input positive and negative values and directions by a pressing operation.

JP 2009-142901 A

  On the other hand, there is a strong demand for the user's instructions to be appropriately transmitted to the robot by pushing or stroking the surface of an object having a plurality of joints such as the arm of the robot.

  The present invention is to solve the above-described problems, and is preferably a control device, a robot, and a control suitable for appropriately changing the shape of a control target by a user operation on the surface of the control target having a plurality of joints. It is an object to provide a method and a program for controlling the input device by a computer.

  A control device according to a first aspect of the present invention includes a base body, a rod-like body having one end connected via a joint capable of rotating and bending with respect to the base body, and an actuator capable of changing the bending angle of the joint. A rotation angle sensor that detects a rotation angle of the joint, and a contact position sensor that detects a contact position of the surrounding surface of the rod-shaped body. And an output unit, which are configured as follows.

  That is, the rotation angle acquisition unit acquires the rotation angle output from the rotation angle sensor.

  On the other hand, the contact angle acquisition unit obtains the center from the contact position output from the contact position sensor based on the direction vector of the perpendicular hung around the central axis in the longitudinal direction of the rod-shaped body and the predetermined position on the peripheral surface of the rod-shaped body. Gets the contact angle formed by the direction vector of the perpendicular to the axis.

  Further, the sum calculation unit calculates the sum of the acquired rotation angle and the acquired contact angle.

Then, the output unit calculates the calculated sum value as follows:
(1) When included in the first angle range, increase the bending angle of the joint;
(2) When included in the second angle range, a control signal for decreasing the bending angle of the joint is output to the control target.

  Moreover, the control apparatus of this invention can be comprised as follows.

  That is, the joint can swing up the rod-shaped body with respect to the base.

  On the other hand, the actuator can change the swing angle of the joint.

Further, the output unit calculates the calculated sum value as follows:
(3) When included in the third angle range, increase the swing angle of the joint,
(4) When included in the fourth angle range, a control signal for reducing the swing angle of the joint is output to the control target.

  The control device of the present invention can be configured as follows.

  That is, the first angle range and the second angle range are point-symmetric with respect to the center of the angle.

  On the other hand, the third angle range and the fourth angle range are point-symmetric with respect to the center of the angle.

  Furthermore, the first angle range and the third angle range are separated by a predetermined separation angle.

  Moreover, the control apparatus of this invention can be comprised as follows.

  That is, when the detected contact position moves around the central axis of the rod-shaped body at a speed within a predetermined range, the output unit outputs a control signal for changing the rotation angle of the joint along the direction of the movement. Output to.

  A robot according to a second aspect of the present invention integrally configures the control target and the control device.

  In the robot according to the present invention, the control target may include a torso, an upper arm, and a forearm, the base may be the torso, and the rod-like body may be the upper arm.

  In the robot according to the present invention, the control target may include a torso, an upper arm, and a forearm, the base body may be the upper arm, and the rod-shaped body may be the forearm.

  A control method according to a third aspect of the present invention includes a base, a rod-like body having one end connected via a joint capable of rotating and bending with respect to the base, and an actuator capable of changing the bending angle of the joint. A control method for controlling a control object comprising: a rotation angle sensor that detects a rotation angle of a joint; and a contact position sensor that detects a contact position of a peripheral surface of the rod-shaped body, the rotation angle acquiring unit, and the contact angle A control device having an acquisition unit, a sum calculation unit, and an output unit executes and is configured as follows.

  That is, in the rotation angle acquisition step, the rotation angle acquisition unit acquires the rotation angle output from the rotation angle sensor.

  On the other hand, in the contact angle acquisition step, the contact angle acquisition unit outputs a direction vector of a perpendicular line around the central axis in the longitudinal direction of the rod-shaped body from the contact position output from the contact position sensor, and the surrounding surface of the rod-shaped body A contact angle formed by a direction vector of a perpendicular line hanging from the predetermined position to the central axis is acquired.

  Further, in the sum calculation step, the sum calculation unit calculates the sum of the acquired rotation angle and the acquired contact angle.

In the output process, the output unit
(1) When the calculated sum value is included in the first angle range, the flexion angle of the joint is increased,
(2) When the calculated sum value is included in the second angle range, a control signal for decreasing the bending angle of the joint is output to the control target.

  A program according to a fourth aspect of the present invention is configured to cause a computer that controls the control target to function as each unit of the control device and to execute each step of the control method.

  The program of the present invention can be recorded on a computer-readable information storage medium such as a compact disk, flexible disk, hard disk, magneto-optical disk, digital video disk, magnetic tape, and semiconductor memory.

  The above program can be distributed and sold via a computer communication network independently of the computer on which the program is executed. The information storage medium can be distributed and sold independently of a computer or a digital signal processor.

  According to the present invention, a control device, a robot, a control method, and an input device that are suitable for appropriately changing the shape of a control target by a user operation on the surface of the control target having a plurality of joints, and the input device are controlled by a computer A program can be provided.

It is a schematic diagram which shows schematic structure of the control apparatus which concerns on this embodiment. It is explanatory drawing which shows the outline | summary of the control object controlled by the control apparatus which concerns on this embodiment. It is a flowchart which shows the flow of the control processing which concerns on this embodiment. It is explanatory drawing showing the angle range in control of the upper arm. It is explanatory drawing showing the angle range in control of a forearm.

  Embodiments of the present invention will be described below. In addition, embodiment described below is for description and does not limit the scope of the present invention. Therefore, those skilled in the art can employ embodiments in which each or all of these elements are replaced with equivalent ones, and these embodiments are also included in the scope of the present invention.

  FIG. 1 is a schematic diagram illustrating a schematic configuration of a control device according to the present embodiment. Hereinafter, a description will be given with reference to FIG.

  As shown in the figure, the control device 101 includes a rotation angle acquisition unit 102, a contact angle acquisition unit 103, a sum calculation unit 104, and an output unit 105. Each of these units controls changes in the shape of a control target such as a robot arm.

  The control device 101 is connected to a control target by wire or wireless, and exchanges various types of information.

  FIG. 2 is an explanatory diagram illustrating an outline of a control target controlled by the control device according to the present embodiment. Hereinafter, a description will be given with reference to FIG.

As shown in the figure, the control object 201 has the following members.
(1) Base 202.
(2) A rod-like body 204 having one end connected to the base body 202 via a joint 203 that can be rotated and bent. In some cases, the joint 203 can be further swung up.
(3) An actuator (not shown) that can change the bending angle of the joint 203. In some cases, it is possible to change the rotation angle or the swing angle of the joint 203.
(4) A rotation angle sensor (not shown) that detects the rotation angle of the joint 203. In addition, in order to monitor the operation of the actuator or the like, a bending angle sensor or a lifting angle sensor that detects a bending angle or a lifting angle of the joint 203 may be provided.
(5) A contact position sensor 205 that detects the contact position of the peripheral surface of the rod-like body 204.

The control target 201 forms a part of the robot, and the base 202, the joint 203, and the rod-like body 204 may have the following combinations.
(A) The body of the robot is a base body 202, the shoulder joint of the robot is a joint 203, and the upper arm of the robot is a rod-like body 204. That is, it is a case of controlling the shoulder and the tip of the robot.
(B) The upper arm of the robot is the base body 202, the elbow joint of the robot is the joint 203, and the forearm of the robot is the rod-like body 204. That is, it is a case where the elbow of the robot and its tip are controlled.

  Hereinafter, the relationship among the bending angle p, the rotation angle q, and the swinging angle r will be described according to the cases (a) and (b). In the following, the state where the arm is extended downward and the palm is directed to the trunk is referred to as a reference posture.

  First, consider the case of (a) above. In the shoulder joint, the robot according to the present embodiment has three degrees of freedom, and can change the bending angle p, the rotation angle q, and the swinging angle r.

  The angle at which the arm is rotated about the rotation axis extending from the shoulder to the left and right corresponds to the swing angle r. In the reference posture, the swing angle r is 0, and when the swing angle r is positively increased from the reference posture, the arm rises forward, and when the swing angle r is decreased negatively from the reference posture, the arm is Will move to.

  When the robot imitates the human body almost faithfully, the swing-up angle r can take any angle from 0 to 360 degrees.

  The bending angle p corresponds to the armpit angle. In the reference posture, the bending angle p is 0, and when the bending angle p is increased from the reference posture, the arm spreads left and right.

  When the robot imitates a human body almost faithfully, the bending angle p can be any angle from 0 to 180 degrees.

  The rotation angle q is an angle for turning the upper arm inward or outward. In the standard posture, the rotation angle q is 0. In the internal rotation in which the upper arm is rotated in the direction in which the palm is directed toward the back in the standard posture, the rotation angle q is increased and the upper arm is rotated in the direction in which the palm is directed in the front direction. In the abduction, the rotation angle q decreases.

  When the robot imitates a human body almost faithfully, the rotation angle q can take an angle of about -90 degrees to 90 degrees.

  Hereinafter, the case (b) will be considered. In the elbow joint, the robot according to the present embodiment has two degrees of freedom and can change the bending angle p and the rotation angle q.

  The bending angle p is an angle at which the forearm is bent at the elbow joint with respect to the upper arm. In the reference posture, the bending angle p is 0, and when the elbow is bent, the bending angle p increases.

  When the robot imitates a human body almost faithfully, the bending angle p can take an angle of about 0 to 170 degrees.

  The rotation angle q is an angle for turning the forearm inward or outward. In the reference posture, the rotation angle q increases in the adduction in which the forearm is rotated in the direction in which the palm is directed to the back in the reference posture, and in the abduction in which the forearm is rotated in the direction in which the palm is directed in the reference posture. Decrease.

  When the robot imitates the human body almost faithfully, the rotation angle q can take an angle of about -135 degrees to +135 degrees.

  As described above, when imitating a human body, a movable range is provided at each angle. However, these movable ranges may be appropriately determined according to the hardware restrictions of the robot.

  As described above, the contact position sensor 205 that detects the contact position touched by the user is disposed on the peripheral surface of the rod-shaped body 204, that is, the entire circumference of the side surface of the rod-shaped body 204 in the longitudinal direction.

  In this embodiment, in order to express the contact position by an angle around the central axis 211 in the longitudinal direction, a concept of a contact angle s is introduced.

What is the contact angle s?
(1) a direction vector 223 of a perpendicular line 222 that hangs from the contact position 221 detected by the contact position sensor 205 to the central axis 211;
(2) a direction vector 225 of a perpendicular 224 hanging from a predetermined reference position 215 on the peripheral surface of the rod-shaped body 204 to the central axis 211;
Is the angle formed by

  In both cases (a) and (b), the reference position 215 is set on the surface on the back side of the arm when the arm is in the reference posture, and the direction of the adduction of the arm is a positive direction. To do. That is, the same rotation direction as the positive direction of the rotation angle q is the positive direction of the contact angle s.

  For example, when the arm is in the standard posture and the arm is pushed from the right side outside, the contact angle s is about 90 degrees.

  The contact position sensor 205 has a sheet-like shape having a two-dimensional spread, and is attached to the surface of the rod-like body 204 to detect which part of the sheet is pressed by the user. Therefore, each position of the contact position sensor 205 and “an angle formed by a direction vector of a perpendicular extending from the corresponding position to the central axis 211 and a direction vector of a perpendicular extending from the reference position 215 to the central axis 211”. If the correlation is made in advance by actual measurement or the like, the contact angle s can be obtained from the sensed contact position.

  In the present embodiment, the process for obtaining the contact angle s from the contact position is performed by the control device 101 as described later.

  In this embodiment, instead of using the posture information of the entire robot, the rotation angle q and the contact angle s are adopted as the local posture information of the robot, and based on these, the local posture and bending of the robot are adopted. One of the features is that the angle p and the swing angle r are easily controlled.

  In addition, by preparing a plurality of control devices 101 and driving actuators that change the angles of the joints 203 of the robot and changing their bending, rotation, and swing angles, the posture of the robot can be changed. . By configuring these control devices 101 and the control target 201 integrally, an autonomous robot can be configured.

  At this time, the control processing executed in each control device 101 can be executed in parallel or sequentially by operating a predetermined program on a computer that controls the robot.

  FIG. 3 is a flowchart showing a flow of control processing according to the present embodiment. Hereinafter, description will be given with reference to FIG. 1 and FIG. This processing represents one unit of processing repeatedly executed for each joint 203 of the robot. Therefore, even if this process is once completed, it is generally configured so that this process is started, and other processes and processes for other joints 203 are appropriately executed between the end and start of this process. Is.

  Now, when this process is started, the rotation angle acquisition unit 102 acquires the rotation angle q output from the rotation angle sensor of the measurement target 201 (step S301).

  The rotation angle q is acquired by appropriately converting the value acquired from the rotation angle sensor so that the reference position, the sign of the rotation direction, and the like match the above settings.

  Next, the contact angle acquisition unit 103 determines whether or not a contact is made from the contact position sensor 205 of the measurement target 201 and the type of the contact if it is touched (step S302). If it is not in contact (step S302; No), this process is terminated.

  If it is touched like stroking (step S302; stroking), a control signal for changing the rotation angle q of the rod-shaped body 204 by a predetermined angle is output in accordance with the stroking direction (step S303). finish.

  Touching as if stroking means that the contact position has moved within the predetermined speed range on the surface of the contact position sensor 205.

  In this embodiment, the rotation angle q is changed to change the direction of the rod-shaped body 204 around the central axis 211 when the surface of the rod-shaped body 204 is boiled in a direction that is twisted with the central axis 211. Limited.

  In addition, the amount by which the rotation angle q is changed may be adjusted according to the difference in angle between the boiled direction and the central axis 211, and the angle between the boiled direction and the central axis 211 is a right angle within a predetermined error range. It is also possible to change the rotation angle q only when close to, and simply end this process otherwise.

  On the other hand, if the contact is made so as to press (step S302; press), the contact angle s is acquired from the contact position output from the contact position sensor 205 (step S312).

  The contact so as to press means contact that simply presses in the normal direction of the surface of the contact position sensor 205. That is, even when the contact position on the surface of the contact position sensor 205 changes, the contact position sensor 205 is pressed when the speed is less than the predetermined speed range (or a threshold smaller than the predetermined speed). It will be judged.

  The rotation angle acquisition unit 102 and the contact angle acquisition unit 103 are realized by a CPU (Central Processing Unit) included in a computer monitoring an I / O port (Input / Output port) that communicates with an external sensor. The

  Further, the association between the contact position and the contact angle s is obtained by referring to a table stored in a hard disk or the like included in the computer or by performing a predetermined calculation on the output of the contact position sensor 205.

  Furthermore, the sum calculation unit 104 calculates the sum (q + s) of the acquired rotation angle q and the acquired contact angle s (step S313). In this embodiment, the actuator is controlled based on local information such as the sum (q + s) of the rotation angle q and the contact angle s, and the angle of the joint 203 of the robot is changed. The calculation of the sum is executed by the CPU.

  Then, the output unit 105 checks whether the calculated sum (q + s) belongs to one of the first angle range, the second angle range, the third angle range, and the fourth angle range (step S314).

  When belonging to the first angle range (step S314; first), the output unit 105 outputs a control signal for increasing the bending angle p of the joint 203 by a predetermined angle (step S315), and ends this processing. To do.

  When belonging to the second angle range (step S314; second), a control signal for decreasing the bending angle p of the joint 203 by a predetermined angle is output (step S316), and this process is terminated.

  When belonging to the third angle range (step S314; third), a control signal for increasing the swing angle r of the joint 203 by a predetermined angle is output (step S317), and this process is terminated.

  When belonging to the fourth angle range (step S314; fourth), a control signal for decreasing the swing angle r of the joint 203 by a predetermined angle is output (step S318), and this process is terminated.

  If it does not belong to any of the first to fourth angle ranges (step S314; other), this process is terminated.

  FIG. 4 is an explanatory diagram showing an angle range in the control of the upper arm (a). Hereinafter, a description will be given with reference to FIG.

  On the right side of the figure, (q + s) = 0 degree, on the upper side of the figure, (q + s) = 90 degrees, on the left side of the figure, (q + s) = 180 degrees, and on the lower side of the figure, (q + s) = 270 degrees are represented respectively.

  When the robot is in the basic posture, the rotation angle q is 0. However, when the robot changes its posture, the rotation angle q also changes.

  When the robot is in the basic posture, (q + s) being around 270 degrees means that the upper arm of the robot is pushed out from between the body and the body. Therefore, the upper arm of the robot is raised to the side by increasing the bending angle p. For this reason, the first angle range 401 is disposed in the vicinity of 270 degrees of (q + s).

  When the upper arm is spread laterally in this way, (q + s) is near 90 degrees, which means that the upper arm is pushed from the side to return to the “careful” posture. To do. Therefore, the bending angle p is decreased and the upper arm of the robot is lowered from the side. For this reason, the second angle range 402 is arranged in the vicinity of (q + s) 90 degrees.

  As shown in the figure, the first angle range 401 and the second angle range 402 are in a point-symmetric relationship with respect to the center 411.

  When the robot is in the basic posture, (q + s) being near 0 degrees means that the upper arm of the robot is pushed from the back side. Therefore, the upper arm of the robot is raised by increasing the raising angle r. Therefore, the third angle range 403 is arranged in the vicinity of 0 degree of (q + s).

  On the other hand, when the robot is in the basic posture, (q + s) is around 180 degrees means that the upper arm of the robot is pushed from the abdomen (front side). Therefore, the upper arm of the robot is swung down by decreasing the swing angle r. Therefore, the fourth angle range 404 is arranged in the vicinity of (q + s) 180 degrees.

  In the present embodiment, the third angle range 403 and the fourth angle range 404 are point-symmetric with respect to the center 411.

  Further, adjacent ones of the first angle range 401, the second angle range 402, the third angle range 403, and the fourth angle range 404 are separated from each other by a predetermined separation angle 405.

  The separation angle 405 provides a dead zone and prevents the robot operation from changing dramatically due to a minute difference in contact position.

  As shown in the figure, a separation angle 405 of about 10 degrees is sufficient, but can be changed as appropriate according to the diameter size of the upper arm, the ability of the user, and the like.

  FIG. 5 is an explanatory diagram showing an angle range in the control of the forearm (b). Hereinafter, a description will be given with reference to FIG.

  The direction shown in this figure is the same as that in FIG. 4, but in this figure, only the first angle range 401 and the second angle range 402 are arranged, and the third angle range 403 and the fourth angle range are arranged. The range 404 is not prepared. This is because a change in the swing angle r is not prepared for the elbow joint in the control of the forearm.

  Now, let us consider a case where the upper arm (and the forearm) is simply spread laterally by operating the upper arm from the basic posture. At this time, the palm is directed toward the ground. Accordingly, when the vicinity of (q + s) = 90 degrees of the forearm is pressed, the elbow is bent and the bending angle p increases, so the first angle range 401 is arranged in this range.

  The second angle range 401 is generally arranged so as to be point-symmetric with respect to the center 411.

  In addition, regarding the bending angle p, the rotation angle q, and the swing angle r, these angle ranges can be appropriately changed depending on which direction is a reference direction and which direction is a positive rotation. .

  Thus, by appropriately setting the first to fourth angle ranges 401, 402, 403, 404 and the separation angle 405 according to the reference position and reference direction set for each control object 201, the robot's In addition to the upper arm and forearm, other members such as the head and legs can be controlled so that the orientation of the member changes in a direction intuitive to the user when the user presses the surface of the member. It becomes.

  According to the present invention, a control device, a robot, a control method, and an input device that are suitable for appropriately changing the shape of a control target by a user operation on the surface of the control target having a plurality of joints, and the input device are controlled by a computer A program can be provided.

DESCRIPTION OF SYMBOLS 101 Control apparatus 102 Rotation angle acquisition part 103 Contact angle acquisition part 104 Sum calculation part 105 Output part 201 Control object 202 Base body 203 Joint 204 Rod-like body 205 Contact position sensor 211 Center axis 215 Reference position 221 Contact position 222 Vertical line 223 Direction vector 224 Normal line 225 Direction vector 401 First angle range 402 Second angle range 403 Third angle range 404 Fourth angle range 405 Separation angle 411 Center

Claims (9)

A base body, a rod-like body having one end connected via a joint capable of rotating and bending with respect to the base body, an actuator capable of changing the bending angle of the joint, and a rotation angle for detecting the rotation angle of the joint A control device that controls a control object including a sensor and a contact position sensor that detects a contact position of a peripheral surface of the rod-shaped body,
A rotation angle acquisition unit for acquiring a rotation angle output from the rotation angle sensor;
From the contact position output from the contact position sensor, the direction vector of the perpendicular hanging around the central axis in the longitudinal direction of the rod-shaped body, and the direction of the perpendicular hanging from the predetermined position on the peripheral surface of the rod-shaped body to the central axis A contact angle acquisition unit for acquiring a contact angle formed by the vector,
A sum calculator for calculating a sum of the acquired rotation angle and the acquired contact angle;
The calculated sum value is
(1) When included in the first angle range, increase the bending angle of the joint,
(2) A control device comprising: an output unit that outputs a control signal for reducing a bending angle of the joint to the control object when included in the second angle range. The control device according to claim 1,
The joint can swing the rod-shaped body with respect to the base body,
The actuator is capable of changing the swing angle of the joint,
The output unit has a value of the calculated sum,
(3) If included in the third angle range, increase the swing angle of the joint;
(4) When included in the fourth angle range, a control signal that decreases the swing angle of the joint is output to the control target. The control device according to claim 2,
The first angle range and the second angle range are in a point-symmetric relationship with respect to the center of the angle,
The third angle range and the fourth angle range are point-symmetric with respect to the center of the angle,
The first angle range and the third angle range are separated from each other by a predetermined separation angle. The control device according to any one of claims 1 to 3,
When the detected contact position moves around the central axis of the rod-shaped body at a speed within a predetermined range,
A control device that outputs a control signal for changing the rotation angle of the joint along the direction of movement to the control target.   5. A robot comprising the control object and the control device according to claim 1 integrally. The robot according to claim 5,
The control target includes a torso, an upper arm, and a forearm, the base is the torso, and the rod-like body is the upper arm. The robot according to claim 5,
The robot to be controlled has a torso, an upper arm, and a forearm, the base is the upper arm, and the rod-like body is the forearm. A base body, a rod-like body having one end connected via a joint capable of rotating and bending with respect to the base body, an actuator capable of changing the bending angle of the joint, and a rotation angle for detecting the rotation angle of the joint A control method executed by a control device that controls a control target including a sensor and a contact position sensor that detects a contact position of a peripheral surface of the rod-shaped body, the control device including a rotation angle acquisition unit, a contact angle Has an acquisition unit, sum calculation unit, output unit,
A rotation angle acquisition step in which the rotation angle acquisition unit acquires a rotation angle output from the rotation angle sensor;
From the contact position output from the contact position sensor, the contact angle acquisition unit is perpendicular to the longitudinal axis of the rod-shaped body, and the center from the predetermined position on the peripheral surface of the rod-shaped body. A contact angle acquisition step of acquiring a contact angle formed by a direction vector of a perpendicular line hanging from the axis, and
A sum calculation step in which the sum calculator calculates a sum of the acquired rotation angle and the acquired contact angle;
The output unit is
(1) When the calculated sum value is included in the first angle range, the bending angle of the joint is increased,
(2) An output step of outputting a control signal for reducing the flexion angle of the joint to the control target when the calculated sum value is included in a second angle range. Method. A base body, a rod-like body having one end connected via a joint capable of rotating and bending with respect to the base body, an actuator capable of changing the bending angle of the joint, and a rotation angle for detecting the rotation angle of the joint A computer for controlling a control object comprising: a sensor; and a contact position sensor that detects a contact position of a peripheral surface of the rod-shaped body
A rotation angle acquisition unit for acquiring a rotation angle output from the rotation angle sensor;
From the contact position output from the contact position sensor, the direction vector of the perpendicular hanging around the central axis in the longitudinal direction of the rod-shaped body, and the direction of the perpendicular hanging from the predetermined position on the peripheral surface of the rod-shaped body to the central axis A contact angle acquisition unit for acquiring a contact angle formed by the vector,
A sum calculator for calculating a sum of the acquired rotation angle and the acquired contact angle;
The calculated sum value is
(1) When included in the first angle range, increase the bending angle of the joint,
(2) A program that, when included in the second angle range, causes a control signal that decreases the bending angle of the joint to function as an output unit that outputs the control target.

Images